Lidar.h

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>.
 
#pragma once
 
#include "CoreMinimal.h"
#include "Containers/Map.h"
#include "Engine/HitResult.h"
 
#include "Agrarsense/Sensor/Sensor.h"
#include "Agrarsense/Sensor/Lidar/LidarParameters.h"
#include "Agrarsense/Sensor/Lidar/PointData.h"
#include "Agrarsense/Weather/WeatherParameters.h"
 
 
#include "ROSIntegration/Public/sensor_msgs/PointCloud2.h"
 
#include <random>
 
#include "Lidar.generated.h"
 
class AGeoReferencingSystem;
class UNiagaraComponent;
class UWorld;
class AWeather;
 
UCLASS()
class AGRARSENSE_API ALidar : public ASensor
{
    GENERATED_BODY()
 
public:
 
    ALidar(const FObjectInitializer& ObjectInitializer);
 
    void Init(FLidarParameters parameters, bool SimulateSensor = true);
 
    virtual ESensorTypes GetSensorType() const override
    {
        return ESensorTypes::Lidar;
    }
 
    UFUNCTION(BlueprintCallable)
    void ChangeLidarParameters(FLidarParameters newLidarParameters);
 
    UFUNCTION(BlueprintCallable)
    void SetVisualizeLidarParticles(bool Visualize);
 
    UFUNCTION(BlueprintCallable)
    void SetParticleLifeTime(float ParticleLifeTime);
 
    UFUNCTION(BlueprintCallable, BlueprintPure)
    FLidarParameters GetLidarParameters() const
    {
        return LidarParameters;
    }
 
    UFUNCTION(BlueprintCallable)
    UNiagaraComponent* GetNiagaraComponent() const
    {
        return NiagaraComponent;
    }
 
    virtual FString GetParametersAsString() const override
    {
        return StructToString(LidarParameters);
    }
 
    /*
    * Force to clear all containers and starting to 
    * capture pointcloud from the beginning.
    */
    void ForceClearContainers()
    {
        ClearContainers = true;
    }
 
    /*
    * Request saving this frame's pointcloud to disk.
    */
    void SaveCurrentPointCloudToDisk() 
    {
        SaveCurrentPointCloudToDiskRequested = true;
    }
 
protected:
 
    virtual void BeginPlay() override;
 
    virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override;
 
    void CreateLogFile() override;
 
    void SaveLidarMetaDataToDisk(const FString& FileName);
 
private:
 
    friend class ALidarManager;
 
    std::vector<FPointData> SimulateRaycastLidar(const float DeltaTime);
 
    UPROPERTY(VisibleAnywhere, Category = "Sensor")
    UNiagaraComponent* NiagaraComponent = nullptr;
 
    void SetNiagaraRendering(bool Enabled);
 
    void VisualizeLidarParticles(std::vector<FPointData> HitPoints);
 
    void ChangeParameters();
 
    FORCEINLINE bool 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);
 
    void ResetRecordedHits(int32 MaxPointsPerChannel);
 
    FORCEINLINE uint32 GetSemanticData(const FHitResult& HitResult) const;
 
    void SendData(const float DeltaTime);
 
    bool CanSendData(const float DeltaTime);
 
    /*
    * Send this Lidar sensor data to its own ROS topic
    */
    void SendDataToTopic(const std::vector<FPointData>& points);
 
    /*
    * Asynchronously saves Lidar point cloud data to disk
    */
    void SaveDataToDisk();
 
    /*
    * Check if we should add extra processing to the current line trace hit result.
    * @param HitResult - The hit result containing the impact point to which noise will be added.
    * @param LidarBodyLocation - The location of the Lidar sensor
    */
    void AddProcessingToHitResult(FHitResult& HitResult, const FVector& LidarBodyLocation, const bool UseHorizontalNoise, const bool UseSnowTerrainAdjustment);
 
    /*
    * Applies distance noise to the hit result based on the standard deviation specified
    * in the Lidar parameters. The noise is added to the impact point, simulating measurement
    * inaccuracies by perturbing the hit point in the direction from the Lidar sensor towards
    * the detected impact point. This noise mimics real-world sensor errors where measurements
    * can vary around the true value.
    * @param HitResult - The hit result containing the impact point to which noise will be added.
    * @param LidarBodyLocation - The location of the Lidar sensor
    */
    void AddDistanceNoise(FHitResult& HitResult, const FVector& LidarBodyLocation);
 
    /*
    * Applies lateral noise to the hit result, introducing random deviation
    * either vertically or horizontally, perpendicular to the Lidar linetrace origin direction.
    * @param HitResult - The hit result containing the impact point to which noise will be added.
    * @param LidarBodyLocation - The location of the Lidar sensor.
    * @param UseHorizontalNoise - Direction to use.
    */
    void AddLateralNoise(FHitResult& HitResult, const FVector& LidarBodyLocation, const bool UseHorizontalNoise);
 
    /*
    * Adjusts the hit result for terrain snow.
    * @param HitResult - The hit result containing the impact point to which noise will be added.
    * @param LidarBodyLocation - The location of the Lidar sensor
    */
    void AddSnowTerrainAdjustment(FHitResult& HitResult, const FVector& LidarBodyLocation);
 
    /*
    * Updates Niagara particle system particles
    */
    void UpdateLidarParticles(int32 NumberOfHits, const TArray<FVector>& HitPoints, const TArray<FLinearColor>& Colors);
 
    /*
    * Callback event from AWeather when weather parameters has been changed.
    */
    UFUNCTION()
    void OnWeatherChanged(FWeatherParameters WeatherParams);
 
    UPROPERTY()
    AGeoReferencingSystem* GeoReferencingSystem = nullptr;
 
    UPROPERTY(VisibleAnywhere, Category = "Sensor")
    FLidarParameters LidarParameters;
 
    FLidarParameters TempLidarParameters;
 
    FWeatherParameters CurrentWeatherParameters;
 
    UPROPERTY()
    TMap<FName, FColor> SemanticColors;
 
    TSharedPtr<ROSMessages::sensor_msgs::PointCloud2> PointCloudMessage;
 
    std::vector<std::vector<FPointData>> PreProcessedHitImpactPoints;
 
    std::vector<std::vector<FPointData>> Points;
    std::vector<FPointData> PointsFlattened;
 
    std::vector<float> LaserAngles;
 
    std::random_device RandomDevice;
    std::mt19937 Generator;
 
    bool NeedToSavePointCloudWithoutNoise = false;
    bool LidarParametersChanged = false;
    bool SendDataEveryFrame = false;
    
    const FName TerrainTag = "Terrain";
 
    float CurrentHorizontalAngle = 0.0f;
    float SensorHzFrequency = 0.1f;
    float SensorHzTimer = 0.0f;
 
    float SnowHeightOffset = 0.0f;
 
    int32 LidarSaves = 0;
 
    uint32 CachedNoneColor;
 
    uint32 CachedSnowflakeColor;
 
    bool SaveCurrentPointCloudToDiskRequested = false;
 
    bool ClearContainers = true;
};