Lidar.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 "Lidar.h"
 
#include "LidarNoiseModel.h"
#include "Agrarsense/Logging/SimulatorLog.h"
#include "Agrarsense/Utils/PointcloudUtilities.h"
#include "Agrarsense/Utils/PlatformUtilities.h"
#include "Agrarsense/Game/AgrarsenseStatics.h"
#include "Agrarsense/Sensor/Lidar/LidarManager.h"
#include "Agrarsense/Sensor/Lidar/PointData.h"
#include "Agrarsense/Weather/Weather.h"
#include "Agrarsense/Utils/CoordinateConversionUtilities.h"
 
#include "GeoReferencingSystem.h"
#include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h"
#include "NiagaraDataInterfaceArrayFunctionLibrary.h"
#include "Math/UnrealMathUtility.h"
#include "NiagaraFunctionLibrary.h"
#include "Misc/FileHelper.h"
#include "NiagaraComponent.h"
#include "Components/PrimitiveComponent.h"
#include "Landscape.h"
#include "Async/ParallelFor.h"
#include "Engine/GameViewportClient.h"
#include "Engine/Engine.h"
#include "Async/Async.h"
#include "Engine/World.h"
#include "CollisionQueryParams.h"
 
#include <iterator>
 
ALidar::ALidar(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
    // Lidar sensors are ticked by LidarManager.cpp
    PrimaryActorTick.bCanEverTick = false;
}
 
void ALidar::Init(FLidarParameters parameters, bool SimulateSensor)
{
    LidarParameters = parameters;
    SetSimulateSensor(SimulateSensor);
    ChangeParameters();
    CreateDataSavePath();
    CreateLogFile();
 
    // Initialize PointCloudMessage
    PointCloudMessage.Reset();
    PointCloudMessage = MakeShared<ROSMessages::sensor_msgs::PointCloud2>();
    PointCloudMessage->CreateDefaultPointCloud2Message();
 
    Generator.seed(RandomDevice());
}
 
void ALidar::BeginPlay()
{
    Super::BeginPlay();
 
    UWorld* World = GetWorld();
 
    // Create the SemanticColors TMap
    TMap<FString, FColor> Colors = GetSemanticColors();
    for (const auto& ColorEntry : Colors)
    {
        FName SemanticName = FName(*ColorEntry.Key);
        SemanticColors.Add(SemanticName, ColorEntry.Value);
    }
 
    // Cache "None" and "Snowflake" colors for Semantic
    FColor NoneColor = SemanticColors.FindRef("None");
    FColor SnowflakeColor = SemanticColors.FindRef("Snowflake");
    CachedNoneColor = (NoneColor.R << 16) | (NoneColor.G << 8) | (NoneColor.B << 0);
    CachedSnowflakeColor = (SnowflakeColor.R << 16) | (SnowflakeColor.G << 8) | (SnowflakeColor.B << 0);
 
    // Add this lidar to LidarManager that handles ticking of this sensor in parallel.
    ALidarManager* LidarManager = UAgrarsenseStatics::GetLidarManager(World);
    if (LidarManager)
    {
        LidarManager->AddRaycastLidar(this);
    }
 
    AWeather* Weather = UAgrarsenseStatics::GetWeatherActor(World);
    if (Weather)
    {
        CurrentWeatherParameters = Weather->GetCurrentWeather();
        Weather->OnWeatherChanged.AddUniqueDynamic(this, &ALidar::OnWeatherChanged);
    }
 
    // Load pointcloud Niagara particle system
    UNiagaraSystem* NS = LoadObject<UNiagaraSystem>(nullptr, TEXT("/Game/Agrarsense/Particles/PointCloud/NS_PointCloudSemanticFast"), nullptr, LOAD_None, nullptr);
    if (NS)
    {
        NiagaraComponent = UNiagaraFunctionLibrary::SpawnSystemAtLocation(World, NS, GetActorLocation());
        if (NiagaraComponent)
        {
            // Hide this NiagaraComponent for all Camera sensors.
            HideComponentForAllCameras(Cast<UPrimitiveComponent>(NiagaraComponent));
 
            NiagaraComponent->SetAffectDynamicIndirectLighting(false);
            NiagaraComponent->SetAffectDistanceFieldLighting(false);
            NiagaraComponent->SetCastContactShadow(false);
            NiagaraComponent->SetCastShadow(false);
 
            SetParticleLifeTime(2.0f);
        }
    }
#if WITH_EDITOR
    else
    {
        UE_LOG(LogTemp, Warning, TEXT("Lidar.cpp: Couldn't find Niagara particle system."));
    }
#endif
}
 
void ALidar::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
 
    UWorld* World = GetWorld();
 
    // Remove this lidar from LidarManager
    ALidarManager* LidarManager = UAgrarsenseStatics::GetLidarManager(World);
    if (LidarManager)
    {
        LidarManager->RemoveRaycastLidar(this);
    }
 
    // Unbind from Weather changed event
    AWeather* Weather = UAgrarsenseStatics::GetWeatherActor(World);
    if (Weather)
    {
        Weather->OnWeatherChanged.RemoveDynamic(this, &ALidar::OnWeatherChanged);
    }
 
    if (NiagaraComponent)
    {
        SetNiagaraRendering(false);
 
        // Destroy component
        NiagaraComponent->UnregisterComponent();
        NiagaraComponent->DestroyComponent();
        NiagaraComponent = nullptr;
    }
 
    PointCloudMessage.Reset();
    PreProcessedHitImpactPoints.clear();
    PointsFlattened.clear();
    LaserAngles.clear();
    Points.clear();
}
 
void ALidar::OnWeatherChanged(FWeatherParameters WeatherParams)
{
    CurrentWeatherParameters = WeatherParams;
 
    // Here we recalculate SnowHeightOffset when Weather changes.
    // This is fine-tuned by hand. If the Snow implementation would change meaningfully,
    // we would need to change this implementation as well.
    // See AddSnowTerrainAdjustment() function how SnowHeightOffset is used.
    SnowHeightOffset = CurrentWeatherParameters.SnowAmount * 0.74f;
    SnowHeightOffset *= 1e-2;
}
 
void ALidar::ChangeParameters()
{
    // Ensure certain parameters are valid ranges
    LidarParameters.HorizontalFov = FMath::Clamp(LidarParameters.HorizontalFov, 0.1f, 360.0f);
    LidarParameters.Channels = FMath::Max(LidarParameters.Channels, 1);
    LidarParameters.DistanceNoiseStdDev = FMath::Clamp(LidarParameters.DistanceNoiseStdDev, 0.0f, 1.0f);
    LidarParameters.LateralNoiseStdDev = FMath::Clamp(LidarParameters.LateralNoiseStdDev, 0.0f, 1.0f);
 
    const int32 NumberOfLasers = LidarParameters.Channels;
 
    // Resize arrays
    PreProcessedHitImpactPoints.resize(NumberOfLasers);
    Points.resize(NumberOfLasers);
 
    // Create Lidar linetrace angles
    LaserAngles.clear();
    const float DeltaAngle = NumberOfLasers == 1u ? 0.f : (LidarParameters.UpperFovLimit - LidarParameters.LowerFovLimit) / static_cast<float>(NumberOfLasers - 1);
 
    for (int32 i = 0; i < NumberOfLasers; ++i)
    {
        const float VerticalAngle = LidarParameters.UpperFovLimit - static_cast<float>(i) * DeltaAngle;
        LaserAngles.emplace_back(VerticalAngle);
    }
 
    SendDataToROS = LidarParameters.SendDataToROS;
    SensorHzFrequency = 1.0f / LidarParameters.RotationFrequency;
    SendDataEveryFrame = !LidarParameters.SendDataAtRotationFrequency;
    
    if (LidarParameters.PointsPerSecond > 1000000)
    {
        SimulatorLog::Log("Lidar sensor: It's highly advisable to turn off VisualizePointcloud when PointsPerSecond exceed 1 million.");
 
        /*if (LidarParameters.PointsPerSecond > 2000000)
        {
            LidarParameters.VisualizePointcloud = false;
            SimulatorLog::Log("Lidar sensor: VisualizePointcloud has been set to false due PointsPerSecond exceeding 2 million.");
        }*/
    }
 
    SetVisualizeLidarParticles(LidarParameters.VisualizePointcloud);
    CreateROSTopic();
}
 
void ALidar::SetVisualizeLidarParticles(bool Visualize)
{
    SetNiagaraRendering(Visualize);
}
 
void ALidar::SetNiagaraRendering(bool Enabled)
{
    if (NiagaraComponent)
    {
        if (!Enabled)
        {
            TArray<FVector> EmptyHitpoints;
            TArray<FLinearColor> EmptyColors;
            UpdateLidarParticles(0, EmptyHitpoints, EmptyColors);
        }
 
        NiagaraComponent->SetRenderingEnabled(Enabled);
    }
}
 
void ALidar::UpdateLidarParticles(int32 NumberOfHits, const TArray<FVector>& HitPoints, const TArray<FLinearColor>& Colors)
{
    if (NiagaraComponent)
    {
        NiagaraComponent->SetVariableFloat(NiagaraPointsFloat, float(NumberOfHits));
        NiagaraComponent->SetVariableInt(NiagaraPointsInt, NumberOfHits);
        UNiagaraDataInterfaceArrayFunctionLibrary::SetNiagaraArrayVector(NiagaraComponent, NiagaraHitPoints, HitPoints);
        UNiagaraDataInterfaceArrayFunctionLibrary::SetNiagaraArrayColor(NiagaraComponent, NiagaraHitColors, Colors);
    }
}
 
void ALidar::SetParticleLifeTime(float ParticleLifeTime)
{
    if (NiagaraComponent)
    {
        NiagaraComponent->SetVariableFloat(FName("User.LifeTime"), ParticleLifeTime);
    }
}
 
std::vector<FPointData> ALidar::SimulateRaycastLidar(const float DeltaTime)
{
    TRACE_CPUPROFILER_EVENT_SCOPE(ALidar::SimulateRaycastLidar);
 
    std::vector<FPointData> HitPoints;
 
    if (!CanSimulateSensor())
    {
        return HitPoints;
    }
 
    // Get sensor position and rotation
    const FTransform& ActorTransform = GetTransform();
    const FVector LidarBodyLocation = ActorTransform.GetLocation();
    const FRotator LidarBodyRotation = ActorTransform.Rotator();
    const FQuat LidarQuat = FQuat(LidarBodyRotation);
 
    // Prepare needed variables for processing
    const int32 ChannelCount = LidarParameters.Channels;
    const int32 PointsToScanWithOneChannel = FMath::RoundHalfFromZero(LidarParameters.PointsPerSecond * DeltaTime / float(ChannelCount));
    const float HorizontalAngle = CurrentHorizontalAngle;
    const float AngleDistanceOfTick = LidarParameters.RotationFrequency * LidarParameters.HorizontalFov * DeltaTime;
    const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneChannel;
    const float Range = LidarParameters.Range * 100; // to meters
 
    const FColor Color = FColor(255, 255, 255);
    const int32 RgbDefault = (Color.R << 16) | (Color.G << 8) | (Color.B << 0);
 
    FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this);
    TraceParams.bReturnPhysicalMaterial = false;
    TraceParams.bReturnFaceIndex = false;
 
    // Note. can become very expensive with high vertice meshes.
    // Any very high vertice mesh should have simpler collision mesh (Tree for example)
    TraceParams.bTraceComplex = LidarParameters.UseComplexCollisionTrace;
 
    // Check if we are going to add any noise
    const bool ShouldAddNoise = (LidarParameters.UseLidarNoiseModel && CurrentWeatherParameters.IsLidarNoiseModelCondition())
        || LidarParameters.DistanceNoiseStdDev != 0.0f
        || LidarParameters.LateralNoiseStdDev != 0.0f;
 
    // Check if separate point cloud should be saved without the noise
    NeedToSavePointCloudWithoutNoise = ShouldAddNoise
        && LidarParameters.SaveDataToDisk 
        && LidarParameters.SavePointcloudWithoutNoiseModel;
 
    const bool Semantic = LidarParameters.Semantic;
    const bool UseSnowTerrainAdjustment = LidarParameters.UseTerrainSnowHitAdjustment && CurrentWeatherParameters.IsWinterSnowCondition();
    const bool NeedsExtraProcessing = UseSnowTerrainAdjustment || LidarParameters.DistanceNoiseStdDev > 0.0f || LidarParameters.LateralNoiseStdDev > 0.0f;
    const bool UseLidarNoiseModel = LidarParameters.UseLidarNoiseModel;
 
    const float HorizontalFOV = LidarParameters.HorizontalFov;
    const float HorizontalFOVHalf = HorizontalFOV / 2;
 
    const UWorld* World = GetWorld();
 
    ResetRecordedHits(PointsToScanWithOneChannel);
 
    // Loop Lidar channels in Parallel
    ParallelFor(ChannelCount, [&](int32 Channel)
    {
        std::vector<FPointData>& ChannelPoints = Points[Channel];
 
        int32 RGB = RgbDefault;
        const bool UseHorizontalNoise = FMath::RandBool();
 
        FHitResult HitResult;
 
        const float VerticalAngle = LaserAngles[Channel];
        const float VerticalRad = FMath::DegreesToRadians(VerticalAngle);
        const float VCos = FMath::Cos(VerticalRad);
        const float VSin = FMath::Sin(VerticalRad);
 
        float CurrentHorizontalAngle = HorizontalAngle;
 
        // Loop through this channel linetraces
        for (int32 i = 0; i < PointsToScanWithOneChannel; i++)
        {
            // Calculate the EndTrace point for this linetrace
            const float LaserHorizontalAngle = CurrentHorizontalAngle - HorizontalFOVHalf;
            const float HorizontalRad = FMath::DegreesToRadians(LaserHorizontalAngle);
            const FVector LocalDirection(VCos * FMath::Cos(HorizontalRad), VCos * FMath::Sin(HorizontalRad), VSin);
            const FVector EndTrace = LidarBodyLocation + Range * LidarQuat.RotateVector(LocalDirection);
 
            if (ShootLaser(World, HitResult, EndTrace, TraceParams, LidarBodyLocation, Channel, Semantic, RgbDefault, UseLidarNoiseModel))
            {
                if (Semantic)
                {
                    RGB = GetSemanticData(HitResult);
                }
 
                if (NeedsExtraProcessing)
                {
                    AddProcessingToHitResult(HitResult, LidarBodyLocation, UseHorizontalNoise, UseSnowTerrainAdjustment);
                }
 
                // Add HitResult impact point to the current channel's point list
                const FVector& HitPoint = HitResult.ImpactPoint;
                ChannelPoints.emplace_back(HitPoint.X, HitPoint.Y, HitPoint.Z, RGB);
            }
 
            CurrentHorizontalAngle += AngleDistanceOfLaserMeasure;
            if (CurrentHorizontalAngle >= HorizontalFOV) 
            {
                CurrentHorizontalAngle -= HorizontalFOV;
            }   
        }
    }, EParallelForFlags::Unbalanced);
 
    CurrentHorizontalAngle = FMath::Fmod(HorizontalAngle + AngleDistanceOfTick, LidarParameters.HorizontalFov);
 
    SendData(DeltaTime);
 
    if (LidarParametersChanged)
    {
        LidarParameters = TempLidarParameters;
        LidarParametersChanged = false;
        ChangeParameters();
    }
 
    if (ClearContainers && LidarParameters.SendDataToCombinedROSTopic)
    {
        return PointsFlattened;
    }
 
    return HitPoints;
}
 
void ALidar::AddProcessingToHitResult(FHitResult& HitResult, const FVector& LidarBodyLocation, const bool UseHorizontalNoise, const bool UseSnowTerrainAdjustment)
{
    if (UseSnowTerrainAdjustment)
    {
        AddSnowTerrainAdjustment(HitResult, LidarBodyLocation);
    }
 
    if (LidarParameters.DistanceNoiseStdDev > 0.0f)
    {
        AddDistanceNoise(HitResult, LidarBodyLocation);
    }
 
    if (LidarParameters.LateralNoiseStdDev > 0.0f)
    {
        AddLateralNoise(HitResult, LidarBodyLocation, UseHorizontalNoise);
    }
}
 
void ALidar::AddDistanceNoise(FHitResult& HitResult, const FVector& LidarBodyLocation)
{
    std::normal_distribution<float> Distribution(0.0f, LidarParameters.DistanceNoiseStdDev);
    float NoiseValue = Distribution(Generator);
    const FVector DirectionToSensor = ((LidarBodyLocation * 1e-2) - HitResult.ImpactPoint).GetSafeNormal();
    const FVector Noise = DirectionToSensor * NoiseValue;
    HitResult.ImpactPoint += Noise;
}
 
void ALidar::AddLateralNoise(FHitResult& HitResult, const FVector& LidarBodyLocation, const bool UseHorizontalNoise)
{
    const FVector ForwardVector = (HitResult.ImpactPoint - LidarBodyLocation).GetSafeNormal();
    const FVector RightVector = FVector::CrossProduct(ForwardVector, FVector::UpVector).GetSafeNormal();
 
    FVector ChosenDirection;
    if (UseHorizontalNoise)
    {
        ChosenDirection = RightVector;
    }
    else
    {
        FVector UpVector = FVector::CrossProduct(ForwardVector, RightVector).GetSafeNormal();
        ChosenDirection = UpVector;
    }
 
    std::normal_distribution<float> Distribution(0.0f, LidarParameters.LateralNoiseStdDev);
    const float NoiseValue = Distribution(Generator);
 
    HitResult.ImpactPoint += ChosenDirection * NoiseValue;
}
 
void ALidar::AddSnowTerrainAdjustment(FHitResult& HitResult, const FVector& LidarBodyLocation)
{
    AActor* HitActor = HitResult.GetActor();
 
    // Terrain should have tag "Terrain" in order to make this work.
    if (HitActor && HitActor->Tags.Contains(TerrainTag))
    {
        // Scale the Z value of ImpactPoint based on SnowHeightOffset.
        HitResult.ImpactPoint.Z += SnowHeightOffset;
    }
}
 
void ALidar::VisualizeLidarParticles(std::vector<FPointData> HitPoints)
{
    TRACE_CPUPROFILER_EVENT_SCOPE(ALidar::VisualizeLidarParticles);
 
    if (!LidarParameters.VisualizePointcloud || !NiagaraComponent || HitPoints.empty())
    {
        return;
    }
 
    if (GEngine && GEngine->GameViewport && GEngine->GameViewport->bDisableWorldRendering)
    {
        // If world rendering (Spectator camera) is disabled, no need to update the particle system.
        return;
    }
 
    const bool Semantic = LidarParameters.Semantic;
    const int32 NumberOfHits = HitPoints.size();
 
    TArray<FVector> HitLocations;
    HitLocations.SetNumUninitialized(NumberOfHits);
 
    TArray<FLinearColor> Colors;
    Colors.Init(FLinearColor::Green, NumberOfHits);
 
    FVector* RESTRICT HitLocationPtr = HitLocations.GetData();
    FLinearColor* RESTRICT ColorsPtr = Colors.GetData();
    const FPointData* RESTRICT HitPointPtr = HitPoints.data();
 
    for (int32 i = 0; i < NumberOfHits; ++i)
    {
        const FPointData& Point = HitPointPtr[i];
 
        HitLocationPtr[i].X = Point.X;
        HitLocationPtr[i].Y = -Point.Y;
        HitLocationPtr[i].Z = Point.Z;
 
        if (Semantic)
        {
            const uint32 RGB = Point.RGB;
            const uint8 R = (RGB >> 16) & 0xFF;
            const uint8 G = (RGB >> 8) & 0xFF;
            const uint8 B = RGB & 0xFF;
 
            ColorsPtr[i].R = R / 255.0f;
            ColorsPtr[i].G = G / 255.0f;
            ColorsPtr[i].B = B / 255.0f;
        }
    }
 
    UpdateLidarParticles(NumberOfHits, HitLocations, Colors);
}
 
void ALidar::SendData(const float DeltaTime)
{
    if (!CanSendData(DeltaTime))
    {
        return;
    }
 
    // Preallocate PointsFlattened size
    // and fill PointsFlattened
    size_t TotalSize = 0;
    for (const std::vector<FPointData>& Vec : Points)
    {
        TotalSize += Vec.size();
    }
    PointsFlattened.reserve(TotalSize);
 
    for (std::vector<FPointData>& HitPoints : Points)
    {
        PointsFlattened.insert(PointsFlattened.end(), std::make_move_iterator(HitPoints.begin()), std::make_move_iterator(HitPoints.end()));
    }
 
    if (LidarParameters.SaveDataToDisk || SaveCurrentPointCloudToDiskRequested)
    {
        SaveDataToDisk();
 
        if (SaveCurrentPointCloudToDiskRequested) {
            SaveCurrentPointCloudToDiskRequested = false;
        }
    }
 
    if (SendDataToROS) 
    {
        SendDataToTopic(PointsFlattened);
    }
}
 
bool ALidar::CanSendData(const float DeltaTime)
{
    if (SendDataEveryFrame)
    {
        ClearContainers = true;
        return true;
    }
 
    SensorHzTimer += DeltaTime;
    if (SensorHzTimer >= SensorHzFrequency)
    {
        SensorHzTimer = 0.0f;
        ClearContainers = true;
        return true;
    }
    else
    {
        ClearContainers = false;
        return false;
    }
}
 
void ALidar::ChangeLidarParameters(FLidarParameters newLidarParameters)
{
    if (!CanSimulateSensor())
    {
        // We are not currently simulating sensor, we can safely change parameters here
        LidarParameters = newLidarParameters;
        ChangeParameters();
    }
    else
    {
        // Change parameters at the end of the frame
        TempLidarParameters = newLidarParameters;
        LidarParametersChanged = true;
    }
}
 
uint32 ALidar::GetSemanticData(const FHitResult& HitResult) const
{
    auto* HitComponent = HitResult.Component.Get();
 
    if (HitComponent)
    {
        TArray<FName>& CompTags = HitComponent->ComponentTags;
        if (!CompTags.IsEmpty())
        {
            FName Tag = CompTags[0]; // First component tag should be the tag we want.
            FColor Color = SemanticColors.FindRef(Tag);
            return (Color.R << 16) | (Color.G << 8) | (Color.B << 0);
        }
        else
        {
            return CachedNoneColor;
        }
    }
    else
    {
        return CachedSnowflakeColor;
    }
}
 
bool ALidar::ShootLaser(const UWorld* World, FHitResult& HitResult, const FVector& EndTrace, const FCollisionQueryParams& TraceParams,
    const FVector& LidarBodyLocation, const int32 Channel, const bool Semantic, const uint32 RGBDefault, const bool UseLidarNoiseModel)
{
#ifdef ParallelLineTraceSingleByChannel_EXISTS
    // Defined and implemented in our AGRARSENSE Unreal Engine fork
    // This LineTrace method doesn't block the physics scene which improves linetrace performance.
    World->ParallelLineTraceSingleByChannel(
        HitResult,
        LidarBodyLocation,
        EndTrace,
        ECC_GameTraceChannel3,
        TraceParams,
        FCollisionResponseParams::DefaultResponseParam);
#else
    // If not using our fork of the engine, use default LineTrace method.
    World->LineTraceSingleByChannel(
        HitResult,
        LidarBodyLocation,
        EndTrace,
        ECC_GameTraceChannel3,
        TraceParams,
        FCollisionResponseParams::DefaultResponseParam);
#endif
 
    bool BlockingHit = HitResult.IsValidBlockingHit();
    if (!BlockingHit && !UseLidarNoiseModel)
    {
        return false;
    }
 
    // Scale the pointcloud
    HitResult.ImpactPoint *= 1e-2;
 
    // Flip Y axis
    HitResult.ImpactPoint.Y *= -1;
 
    if (NeedToSavePointCloudWithoutNoise)
    {
        // Save Original FHitResult to separate array
        FHitResult OriginalHitResultCopy = HitResult;
        uint32 RGB = RGBDefault;
        if (Semantic)
        {
            // If Lidar is set to Semantic, set point color
            RGB = GetSemanticData(OriginalHitResultCopy);
        }
        const FVector& HitPoint = OriginalHitResultCopy.ImpactPoint;
        FPointData point = { HitPoint.X, HitPoint.Y, HitPoint.Z, RGB };
        PreProcessedHitImpactPoints[Channel].emplace_back(point);
    }
 
    if (UseLidarNoiseModel)
    {
        // Check whether we "hit" snowflake by using LUAS formula
        bool HitSnowFlake = LidarNoiseModel::CheckSnowflakeHit(HitResult, EndTrace, LidarBodyLocation, CurrentWeatherParameters);
        BlockingHit = HitResult.bBlockingHit || HitSnowFlake;
    }
 
    return BlockingHit;
}
 
void ALidar::ResetRecordedHits(int32 MaxPointsPerChannel)
{
    if (!ClearContainers)
    {
        return;
    }
 
    // Visualize Lidar particles on main thread because, 
    // in some instances NiagaraComponent crashes when updating points from background thread.
    std::vector<FPointData> CopiedPoints = PointsFlattened;
    AsyncTask(ENamedThreads::GameThread, [this, CopiedPoints]()
    {
        VisualizeLidarParticles(CopiedPoints);
    });
 
    if (!SendDataEveryFrame)
    {
        // If we only send data at Lidar sensor rotation frequency
        // reserve enough points to fill all points
        MaxPointsPerChannel = LidarParameters.PointsPerSecond / LidarParameters.Channels;
    }
 
    // Clear all containers
    if (LidarParameters.SavePointcloudWithoutNoiseModel)
    {
        for (std::vector<FPointData>& ImpactPoints : PreProcessedHitImpactPoints)
        {
            ImpactPoints.clear();
            ImpactPoints.reserve(MaxPointsPerChannel);
        }
    }
 
    for (std::vector<FPointData>& ImpactPoints : Points)
    {
        ImpactPoints.clear();
        ImpactPoints.reserve(MaxPointsPerChannel);
    }
 
    PointsFlattened.clear();
}
 
void ALidar::SendDataToTopic(const std::vector<FPointData>& points)
{
    UTopic* Topic = GetROSTopic();
 
    if (!IsROSConnected()
        || !Topic
        || !SendDataToROS
        || !PointCloudMessage.IsValid()
        || points.empty())
    {
        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()));
    PointCloudMessage->header.time = FROSTime::Now();
    Topic->Publish(PointCloudMessage);
}
 
void ALidar::SaveDataToDisk()
{
    if (PointsFlattened.size() == 0)
    {
        return;
    }
 
    // Save point cloud to disk
    const FString Filename = FString::Printf(TEXT("%sLidar_%d.ply"), *FileSavePath, LidarSaves);
    UPointcloudUtilities::SaveVectorArrayAsPlyAsync(Filename, PointsFlattened);
    SaveLidarMetaDataToDisk(Filename);
 
    if (NeedToSavePointCloudWithoutNoise)
    {
        // Flatten 2D std::vector to 1D std::vector
        std::vector<FPointData> PointsWithOutNoiseModel;
    
        size_t totalSize = 0;
        for (const std::vector<FPointData>& Vec : PreProcessedHitImpactPoints)
        {
            totalSize += Vec.size();
        }
        PointsWithOutNoiseModel.reserve(totalSize);
 
        for (const std::vector<FPointData>& Vec : PreProcessedHitImpactPoints)
        {
            PointsWithOutNoiseModel.insert(PointsWithOutNoiseModel.end(), Vec.begin(), Vec.end());
        }
 
        // Save pointcloud to disk without any noise
        const FString PreprocessedFilename = FString::Printf(TEXT("%sLidar_%d_without_noise_model.ply"), *FileSavePath, LidarSaves);
        UPointcloudUtilities::SaveVectorArrayAsPlyAsync(PreprocessedFilename, PointsWithOutNoiseModel);
        SaveLidarMetaDataToDisk(PreprocessedFilename);
    }
 
    ++LidarSaves;
}
 
void ALidar::CreateLogFile()
{
    if (IsValid(LogFile))
    {
        // File has already been created, return
        return;
    }
 
    FLogFileSettings Settings;
    Settings.FileCreationOptions = FFileCreationOptions::Overwrite;
    Settings.FileWriteOptions = FFileWriteOptions::Queue;
    Settings.QueueLength = MAX_int32; // Only write the log after destroying the sensor
    Settings.KeepFileOpen = false;
    Settings.Timestamp = false;
    Settings.OverrideFilePath = true;
    Settings.FilePath = FileSavePath;
 
    LogFile = NewObject<ULogFile>(ULogFile::StaticClass());
    if (IsValid(LogFile))
    {
        const FString FileName = "lidar_metadata";
        LogFile->Create(FileName, Settings);
 
        // Write camera metadata first line to explain the values.
        GeoReferencingSystem = AGeoReferencingSystem::GetGeoReferencingSystem(GetWorld());
        if (GeoReferencingSystem)
        {
            WriteToLogFile("timestamp, pointcloud_name, X location, Y location, Z location, yaw rotation, pitch rotation, roll rotation, GPS latitude, GPS longitude, GPS altitude");
        }
        else
        {
            WriteToLogFile("timestamp, pointcloud_name, X location, Y location, Z location, yaw rotation, pitch rotation, roll rotation");
        }
    }
}
 
void ALidar::SaveLidarMetaDataToDisk(const FString& FileName)
{
    if (!IsValid(LogFile))
    {
        // If the log file located in base class is not valid, return here.
        return;
    }
 
    const FVector ActorPosition = GetActorLocation();
    const FRotator ActorRotation = GetActorRotation();
 
    const FString TimeStamp = CreateTimeStampString();
    FString MetaData;
 
    if (GeoReferencingSystem)
    {
        FGeographicCoordinates GeoCoordinates = UCoordinateConversionUtilities::UnrealToGeographicCoordinates(GeoReferencingSystem, ActorPosition);
        MetaData = FString::Printf(TEXT("%s, %s, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.8f, %.8f, %.8f"),
            *TimeStamp, *FileName,
            ActorPosition.X, ActorPosition.Y, ActorPosition.Z,
            ActorRotation.Pitch, ActorRotation.Yaw, ActorRotation.Roll,
            GeoCoordinates.Latitude, GeoCoordinates.Longitude, GeoCoordinates.Altitude);
    }
    else
    {
        MetaData = FString::Printf(TEXT("%s, %s, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f"),
            *TimeStamp, *FileName,
            ActorPosition.X, ActorPosition.Y, ActorPosition.Z,
            ActorRotation.Pitch, ActorRotation.Yaw, ActorRotation.Roll);
    }
 
    // Write to the log file (this is written after sensor is destroyed)
    WriteToLogFile(MetaData);
}