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->header.frame_id = "world";
    PointCloudMessage->is_dense = true;
    PointCloudMessage->fields.SetNum(4);
    PointCloudMessage->height = 1;
 
    PointCloudMessage->fields[0].name = "x";
    PointCloudMessage->fields[0].offset = 0;
    PointCloudMessage->fields[0].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::FLOAT32;
    PointCloudMessage->fields[0].count = 1;
 
    PointCloudMessage->fields[1].name = "y";
    PointCloudMessage->fields[1].offset = 4;
    PointCloudMessage->fields[1].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::FLOAT32;
    PointCloudMessage->fields[1].count = 1;
 
    PointCloudMessage->fields[2].name = "z";
    PointCloudMessage->fields[2].offset = 8;
    PointCloudMessage->fields[2].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::FLOAT32;
    PointCloudMessage->fields[2].count = 1;
 
    PointCloudMessage->fields[3].name = "rgb";
    PointCloudMessage->fields[3].offset = 12;
    PointCloudMessage->fields[3].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::UINT32;
    PointCloudMessage->fields[3].count = 1;
 
    PointCloudMessage->point_step = 16;
}
 
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
 
    SimulateRaycastLidars(DeltaTime);
 
#ifdef WITH_EDITOR
    if (MeasurePerformance && TotalAverageTimer)
    {
        double ms = TotalAverageTimer->PrintAverageTime();
        TimeInMilliseconds = FMath::RoundToFloat(ms * 100.0f) / 100.0f;
    }
#endif
}
 
void ALidarManager::SimulateRaycastLidars(const float DeltaTime)
{
    TRACE_CPUPROFILER_EVENT_SCOPE(ALidarManager::SimulateRaycastLidars);
 
    const int lidarCount = RaycastLidars.size();
 
    if (lidarCount == 0 || !SimulateLidars)
    {
        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());
            {
                flattenedPoints = lidar->SimulateRaycastLidar(DeltaTime);
            }
            PhysicsLock.Release();
#else
            // If not using AGRARSENSE fork no need to lock the physics scene.
            flattenedPoints = lidar->SimulateRaycastLidar(DeltaTime);
#endif
        }
    }
    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)
            {
#ifdef ParallelLineTraceSingleByChannel_EXISTS
                // ParallelLineTraceSingleByChannel_EXISTS Defined and implemented in AGRARSENSE Unreal Engine fork
                // Lock physics scene while linetracing.
                auto PhysicsLock = FPhysicsObjectExternalInterface::LockRead(GetWorld()->GetPhysicsScene());
                {
                    points[lidarIndex] = lidar->SimulateRaycastLidar(DeltaTime);
                }
                PhysicsLock.Release();
#else
                // If not using AGRARSENSE fork no need to lock the physics scene.
                points[lidarIndex] = lidar->SimulateRaycastLidar(DeltaTime);
#endif
            }
        }, EParallelForFlags::Unbalanced);
        
        if (SaveCombinedCloudToDisk || SendCombinedCloudToROS)
        {
            int32 TotalSize = 0;
 
            for (const auto& subVector : points)
            {
                TotalSize += subVector.size();
            }
 
            if (TotalSize != 0)
            {
                flattenedPoints.reserve(TotalSize);
                for (auto& subVector : points)
                {
                    flattenedPoints.insert(flattenedPoints.end(), std::make_move_iterator(subVector.begin()), std::make_move_iterator(subVector.end()));
                }
            }
        }
    }
 
    if (!flattenedPoints.empty())
    {
        if (SaveCombinedCloudToDisk) 
        {
            SaveCombinedPointcloud(flattenedPoints);
        }
        
        SendCombinedPointcloud(flattenedPoints);
    }
}
 
void ALidarManager::SetSimulateLidars(bool bSimulateLidars)
{
    SimulateLidars = 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
    int 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 int pointSize = points.size();
    size_t dataSize = pointSize * sizeof(FPointData);
 
    PointCloudMessage->width = pointSize;
    PointCloudMessage->row_step = dataSize;
    PointCloudMessage->data_ptr = reinterpret_cast<uint8*>(const_cast<FPointData*>(points.data()));
 
    Topic->Publish(PointCloudMessage);
}