Radar.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 "Radar.h"
 
#include "Agrarsense/Game/AgrarsenseStatics.h"
 
#include "NiagaraDataInterfaceArrayFunctionLibrary.h"
#include "NiagaraFunctionLibrary.h"
#include "CollisionQueryParams.h"
#include "Async/ParallelFor.h"
#include "NiagaraComponent.h"
#include "DrawDebugHelpers.h"
 
ARadar::ARadar(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
    PrimaryActorTick.bCanEverTick = true;
}
 
void ARadar::Init(FRadarParameters parameters, bool SimulateSensor)
{
    SetSimulateSensor(SimulateSensor);
    SetRadarParameters(parameters);
 
    World = GetWorld();
 
    CreateROSTopic();
 
    // Load pointcloud Niagara particle system
    UNiagaraSystem* NS = LoadObject<UNiagaraSystem>(nullptr, TEXT("/Game/Agrarsense/Particles/PointCloud/NS_PointCloudFast"), 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);
        }
    }
#if WITH_EDITOR
    else
    {
        UE_LOG(LogTemp, Warning, TEXT("Radar.cpp: Couldn't find Niagara particle system."));
    }
#endif
}
 
void ARadar::BeginPlay()
{
    Super::BeginPlay();
 
    if (!World)
    {
        World = GetWorld();
    }
 
    PrevLocation = GetActorLocation();
 
    // Create RadarMessage
    RadarMessage = MakeShared<ROSMessages::sensor_msgs::PointCloud2>();
    RadarMessage->header.frame_id = "world";
    RadarMessage->height = 1;
    RadarMessage->is_dense = true;
    RadarMessage->fields.SetNum(7);
 
    RadarMessage->fields[0].name = "x";
    RadarMessage->fields[0].offset = 0;
    RadarMessage->fields[0].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::FLOAT32;
    RadarMessage->fields[0].count = 1;
 
    RadarMessage->fields[1].name = "y";
    RadarMessage->fields[1].offset = 4;
    RadarMessage->fields[1].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::FLOAT32;
    RadarMessage->fields[1].count = 1;
 
    RadarMessage->fields[2].name = "z";
    RadarMessage->fields[2].offset = 8;
    RadarMessage->fields[2].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::FLOAT32;
    RadarMessage->fields[2].count = 1;
 
    RadarMessage->fields[3].name = "Range";
    RadarMessage->fields[3].offset = 12;
    RadarMessage->fields[3].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::FLOAT32;
    RadarMessage->fields[3].count = 1;
 
    RadarMessage->fields[4].name = "Velocity";
    RadarMessage->fields[4].offset = 16;
    RadarMessage->fields[4].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::FLOAT32;
    RadarMessage->fields[4].count = 1;
 
    RadarMessage->fields[5].name = "AzimuthAngle";
    RadarMessage->fields[5].offset = 20;
    RadarMessage->fields[5].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::FLOAT32;
    RadarMessage->fields[5].count = 1;
 
    RadarMessage->fields[6].name = "ElevationAngle";
    RadarMessage->fields[6].offset = 28;
    RadarMessage->fields[6].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::FLOAT32;
    RadarMessage->fields[6].count = 1;
 
    RadarMessage->point_step = 56;
}
 
void ARadar::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
 
    HitLocations.Empty();
 
    detections.clear();
    Rays.clear();
 
    if (RadarMessage.IsValid())
    {
        RadarMessage.Reset();
    }
 
    if (NiagaraComponent)
    {
        NiagaraComponent->UnregisterComponent();
        NiagaraComponent->DestroyComponent();
    }
 
    World = nullptr;
}
 
void ARadar::Tick(float DeltaTime)
{
    TRACE_CPUPROFILER_EVENT_SCOPE(ARadar::Tick);
 
    if (!CanSimulateSensor())
    {
        return;
    }
 
    SimulateRadar(DeltaTime);
 
    if (RadarParametersChanged)
    {
        SetRadarParameters(TempRadarParameters);
        RadarParametersChanged = false;
    }
}
 
void ARadar::ChangeRadarParameters(FRadarParameters Parameters)
{
    if (!CanSimulateSensor())
    {
        // We are not currently simulating sensor, we can safely change parameters here
        SetRadarParameters(Parameters);
    }
    else
    {
        // Change parameters at the end of the frame
        TempRadarParameters = Parameters;
        RadarParametersChanged = true;
    }
}
 
void ARadar::SetVisualizeParticles(bool Visualize)
{
    VisualizeParticles = Visualize;
    if (NiagaraComponent)
    {
        NiagaraComponent->SetRenderingEnabled(VisualizeParticles);
    }
}
 
void ARadar::SetRadarParameters(FRadarParameters Parameters)
{
    RadarParameters = Parameters;
 
    SendDataToROS = RadarParameters.SendDataToRos;
    VisualizeParticles = RadarParameters.VisualizeRadarHits;
 
    detections.clear();
    detections.shrink_to_fit();
    detections.reserve(RadarParameters.PointsPerSecond);
}
 
void ARadar::CalculateCurrentVelocity(const float DeltaTime)
{
    const FVector RadarLocation = GetActorLocation();
    CurrentVelocity = (RadarLocation - PrevLocation) / DeltaTime;
    PrevLocation = RadarLocation;
}
 
void ARadar::SimulateRadar(float DeltaTime)
{
    TRACE_CPUPROFILER_EVENT_SCOPE(ARadar::SimulateRadar);
 
    CalculateCurrentVelocity(DeltaTime);
 
    auto TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this);
    TraceParams.bTraceComplex = true;
    TraceParams.bReturnPhysicalMaterial = false;
 
    constexpr float TO_METERS = 1e-2;
    const FTransform& ActorTransform = GetActorTransform();
    const FRotator& TransformRotator = ActorTransform.Rotator();
    const FVector& RadarLocation = GetActorLocation();
    const FVector& ForwardVector = GetActorForwardVector();
    const FVector TransformXAxis = ActorTransform.GetUnitAxis(EAxis::X);
    const FVector TransformYAxis = ActorTransform.GetUnitAxis(EAxis::Y);
    const FVector TransformZAxis = ActorTransform.GetUnitAxis(EAxis::Z);
 
    const float Range = RadarParameters.Range * 100;
    const float HorizontalFOV = RadarParameters.HorizontalFOV;
    const float VerticalFOV = RadarParameters.VerticalFOV;
    const float PointsPerSecond = RadarParameters.PointsPerSecond;
 
    bool DebugLines = false;
    EParallelForFlags Flag = EParallelForFlags::Unbalanced; 
    if (DebugLines) 
    {
        Flag = EParallelForFlags::ForceSingleThread;
    }
 
    // Maximum radar radius in horizontal and vertical direction
    const float MaxRx = FMath::Tan(FMath::DegreesToRadians(HorizontalFOV * 0.5f)) * Range;
    const float MaxRy = FMath::Tan(FMath::DegreesToRadians(VerticalFOV * 0.5f)) * Range;
    const int NumPoints = (int)(PointsPerSecond * DeltaTime);
 
    Rays.clear();
    Rays.resize(NumPoints);
    for (int32 i = 0; i < Rays.size(); i++)
    {
        Rays[i].Radius = static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
        Rays[i].Angle = 0.0f + static_cast<float>(rand()) / (static_cast<float>(RAND_MAX / (2 * PI - 0.0f)));
        Rays[i].Hit = false;
    }
 
    // Linetrace in parallel
    ParallelFor(NumPoints, [&](int32 idx)
    {
        FHitResult OutHit(ForceInit);
        const float Radius = Rays[idx].Radius;
        const float Angle = Rays[idx].Angle;
 
        float Sin, Cos;
        FMath::SinCos(&Sin, &Cos, Angle);
 
        const FVector EndLocation = RadarLocation + TransformRotator.RotateVector({
            Range,
            MaxRx * Radius * Cos,
            MaxRy * Radius * Sin
        });
 
        bool hit = false;
 
#ifdef ParallelLineTraceSingleByChannel_EXISTS
        hit = World->ParallelLineTraceSingleByChannel(
            OutHit,
            RadarLocation,
            EndLocation,
            ECC_GameTraceChannel3,
            TraceParams,
            FCollisionResponseParams::DefaultResponseParam);
#else
        hit = World->LineTraceSingleByChannel(
            OutHit,
            RadarLocation,
            EndLocation,
            ECC_GameTraceChannel3,
            TraceParams,
            FCollisionResponseParams::DefaultResponseParam);
#endif
 
#ifdef UE_BUILD_DEBUG
        if (DebugLines)
        {
            DrawDebugLine(World, RadarLocation, EndLocation, FColor::Green, false, 1.0f);
        }
#endif
 
        const TWeakObjectPtr<AActor> Actor = OutHit.GetActor();
        if (hit && Actor.IsValid())
        {
            auto point = OutHit.ImpactPoint;
            point *= 1e-2;
            Rays[idx].HitPoint = point;
            Rays[idx].Hit = true;
            Rays[idx].RelativeVelocity = CalculateRelativeVelocity(OutHit, RadarLocation, Actor->GetVelocity());
            Rays[idx].AzimuthAndElevation = FMath::GetAzimuthAndElevation(
                (EndLocation - RadarLocation).GetSafeNormal() * Range,
                TransformXAxis,
                TransformYAxis,
                TransformZAxis
            );
            Rays[idx].Distance = OutHit.Distance * TO_METERS;
        }
    }, Flag);
 
 
    if (VisualizeParticles)
    {
        HitLocations.Empty();
    }
 
    // Clear previous detections
    detections.clear();
 
    // Write the detections to the output structure
    for (auto& ray : Rays)
    {
        if (ray.Hit)
        {
            detections.push_back({
                ray.RelativeVelocity,
                (float)ray.AzimuthAndElevation.X,
                (float)ray.AzimuthAndElevation.Y,
                 ray.Distance
            });
 
            if (VisualizeParticles)
            {
                HitLocations.Add(ray.HitPoint);
            }
        }
    }
 
    if (VisualizeParticles && NiagaraComponent)
    {
        int numberOfHits = HitLocations.Num();
        NiagaraComponent->SetVariableFloat(NiagaraPointsFloat, float(numberOfHits));
        NiagaraComponent->SetVariableInt(NiagaraPointsInt, numberOfHits);
        UNiagaraDataInterfaceArrayFunctionLibrary::SetNiagaraArrayVector(NiagaraComponent, NiagaraHitPoints, HitLocations);
    }
 
    SendRadarData();
}
 
float ARadar::CalculateRelativeVelocity(const FHitResult& OutHit, const FVector& RadarLocation, const FVector ActorVelocity)
{
    constexpr float TO_METERS = 1e-2;
 
    const FVector TargetVelocity = ActorVelocity;
    const FVector TargetLocation = (OutHit.ImpactPoint * TO_METERS);
    const FVector Direction = (TargetLocation - RadarLocation).GetSafeNormal();
    const FVector DeltaVelocity = (TargetVelocity - CurrentVelocity);
    const float V = TO_METERS * FVector::DotProduct(DeltaVelocity, Direction);
 
    return V;
}
 
void ARadar::SendRadarData()
{
    UTopic* Topic = GetROSTopic();
 
    if (!SendDataToROS
        || !Topic
        || !RadarMessage.IsValid()
        || detections.empty())
    {
        return;
    }
 
    int detectionsSize = detections.size();
    RadarMessage->width = detectionsSize;
    RadarMessage->row_step = detectionsSize * RadarMessage->point_step;
 
    TUniquePtr<uint8> data(new uint8[RadarMessage->row_step]);
    int pointStep = RadarMessage->point_step;
 
    int i = 0;
    for (const FRadarDetection& detection : detections)
    {
        RadarData params =
        {
          detection.depth * cos(detection.azimuth) * cos(-detection.altitude),
          detection.depth * sin(-detection.azimuth) * cos(detection.altitude),
          detection.depth * sin(detection.altitude),
          detection.depth,
          detection.velocity,
          detection.azimuth,
          detection.altitude
        };
 
        memcpy(data.Get() + i * pointStep, &params, sizeof(params));
        i++;
    }
 
    RadarMessage->data_ptr = data.Get();
 
    Topic->Publish(RadarMessage);
}