OverlapSensor.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 "OverlapSensor.h"
#include "Agrarsense/Vehicle/Vehicle.h"
#include "Agrarsense/InstancedRendering/InstancedActorRenderer.h"
#include "Agrarsense/Game/AgrarsenseStatics.h"
#include "Agrarsense/ActorInterfaces/ActorInformation.h"
#include "Agrarsense/Logging/SimulatorLog.h"
#include "Agrarsense/Components/BoundsVisualizerComponent.h"
 
#include "Components/InstancedStaticMeshComponent.h"
#include "Components/SkeletalMeshComponent.h"
#include "Components/StaticMeshComponent.h"
#include "Engine/SkeletalMesh.h"
#include "Engine/StaticMesh.h"
 
AOverlapSensor::AOverlapSensor(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
    PrimaryActorTick.bCanEverTick = false;
 
    BoundsComponent = CreateDefaultSubobject<UBoundsVisualizerComponent>(TEXT("BoundsComponent"));
    BoundsComponent->SetupAttachment(RootComponent);
    BoundsComponent->SetGenerateOverlapEvents(true);
    BoundsComponent->SetVisibility(false);
    BoundsComponent->SetCollisionEnabled(ECollisionEnabled::QueryOnly);
}
 
void AOverlapSensor::Init(FOverlapSensorParameters InParameters)
{
    Parameters = InParameters;
 
    Vehicle = Cast<AVehicle>(Parameters.OwningActor);
 
    if (Vehicle)
    {
        VehicleName = IActorInformation::Execute_GetActorName(Vehicle);
        VehicleID = IActorInformation::Execute_GetActorID(Vehicle);
    }
 
    UPrimitiveComponent* TriggerComponent = Cast<UPrimitiveComponent>(BoundsComponent);
 
    if (!InParameters.AllChannels)
    {
        const int32 OverlapSensorIndex = 2;
        const ECollisionChannel OverlapSensorChannel = UCollisionProfile::Get()->ConvertToCollisionChannel(false, OverlapSensorIndex);
 
        BoundsComponent->SetCollisionObjectType(OverlapSensorChannel);
        BoundsComponent->SetCollisionResponseToAllChannels(ECollisionResponse::ECR_Ignore);
        BoundsComponent->SetCollisionResponseToChannel(OverlapSensorChannel, ECollisionResponse::ECR_Overlap);
    }
    else
    {
        // Set to use custom collision profile defined in DefaultEngine.ini
        BoundsComponent->SetCollisionProfileName(TEXT("OverlapSensorAll"));
    }
 
    BoundsComponent->SetCollisionEnabled(ECollisionEnabled::QueryOnly);
    BoundsComponent->SetGenerateOverlapEvents(true);
    BoundsComponent->SetRelativeLocation(Parameters.RelativePosition);
 
    if (RootComponent)
    {
        BoundsComponent->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepWorldTransform);
    }
 
    TriggerComponent = BoundsComponent;
 
    if (TriggerComponent)
    {
        SetOverlapBounds(InParameters.Size);
        //SetRootComponent(TriggerComponent);
        SetSimulateSensor(true);
 
        ROSMessages::std_msgs::String StringMsg;
        ROSMessage = MakeShared<ROSMessages::std_msgs::String>(StringMsg);
 
        if (Parameters.CreateROSTopic)
        {
            CreateROSTopic();
        }
    }
 
    if (BoundsComponent)
    {
        FVector Size = FVector(Parameters.Size.X, Parameters.Size.Y, Parameters.Size.Z);
        BoundsComponent->SetRelativeScale3D(Size);
        BoundsComponent->SetRelativeLocation(Parameters.RelativePosition);
 
        if (RootComponent)
        {
            BoundsComponent->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepWorldTransform);
        }
    }
 
    if (Parameters.OwningActor)
    {
        AttachToActor(Parameters.OwningActor, FAttachmentTransformRules::KeepWorldTransform);
    }
 
    CreateLogFile();
}
 
void AOverlapSensor::BeginPlay()
{
    Super::BeginPlay();
 
    BoundsComponent->OnComponentBeginOverlap.AddDynamic(this, &AOverlapSensor::OnOverlapBegin);
    BoundsComponent->OnComponentEndOverlap.AddDynamic(this, &AOverlapSensor::OnOverlapEnd);
}
 
void AOverlapSensor::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
 
    BoundsComponent->OnComponentBeginOverlap.RemoveDynamic(this, &AOverlapSensor::OnOverlapBegin);
    BoundsComponent->OnComponentEndOverlap.RemoveDynamic(this, &AOverlapSensor::OnOverlapEnd);
}
 
void AOverlapSensor::SetOverlapBounds(const FVector& NewSize)
{
    Parameters.Size = NewSize;
 
    if (BoundsComponent)
    {
        FVector Size = FVector(Parameters.Size.X, Parameters.Size.Y , Parameters.Size.Z);
        BoundsComponent->SetRelativeScale3D(Size);
    }
 
    FString ActorID = IActorInformation::Execute_GetActorID(this);
 
    FString Msg = FString::Printf(TEXT("OverlapSensor with ID: %s changed overlap bounds to: %s"), *ActorID, *NewSize.ToString());
    SimulatorLog::Log(Msg);
}
 
void AOverlapSensor::SetVisualizeOverlapArea(bool Visualize)
{
    IsVisible = Visualize;
 
    if (BoundsComponent)
    {
        BoundsComponent->SetVisibility(IsVisible);
    }
}
 
void AOverlapSensor::SetOverlapRelativePosition(const FVector& Vector)
{
    Parameters.RelativePosition = Vector;
 
    if (BoundsComponent)
    {
        BoundsComponent->SetRelativeLocation(Parameters.RelativePosition);
    }
 
    FString Msg = FString::Printf(TEXT("OverlapSensor.cpp: Changed overlap relative position to: %s"), *Vector.ToString());
    SimulatorLog::Log(Msg);
}
 
void AOverlapSensor::SetOverlapResponseToAllChannels(const ECollisionResponse Response)
{
    if (Response != ECollisionResponse::ECR_Overlap && Response != ECollisionResponse::ECR_Ignore)
    {
        // If Response is neither ECR_Overlap nor ECR_Ignore, return
        return;
    }
 
    if (BoundsComponent)
    {
        BoundsComponent->SetCollisionResponseToAllChannels(Response);
    }
}
 
void AOverlapSensor::OnOverlapBegin(UPrimitiveComponent* OverlappedComponent, AActor* OtherActor, UPrimitiveComponent* OtherComp, int32 OtherBodyIndex, bool bFromSweep, const FHitResult& SweepResult)
{
    if (IsValidOverlap(OtherActor, OtherComp))
    {
        BuildAndSendMessage("Overlap Begin", OtherActor, OtherComp, OtherBodyIndex);
    }
}
 
void AOverlapSensor::OnOverlapEnd(UPrimitiveComponent* OverlappedComponent, AActor* OtherActor, UPrimitiveComponent* OtherComp, int32 OtherBodyIndex)
{
    if (IsValidOverlap(OtherActor, OtherComp))
    {
        BuildAndSendMessage("Overlap End", OtherActor, OtherComp, OtherBodyIndex);
    }
}
 
bool AOverlapSensor::IsValidOverlap(const AActor* OtherActor, const UPrimitiveComponent* OtherComp) const
{
    if (!IsValid(OtherComp)) return false;
    if (OtherActor && OtherActor->IsA<AOverlapSensor>()) return false;
    if (OtherComp->IsA<UBoundsVisualizerComponent>()) return false;
 
    // Only consider overlaps with these component types
    return OtherComp->IsA<UInstancedStaticMeshComponent>() ||
        OtherComp->IsA<UStaticMeshComponent>() ||
        OtherComp->IsA<USkeletalMeshComponent>();
}
 
void AOverlapSensor::BuildAndSendMessage(const FString& Prefix, AActor* Actor, UPrimitiveComponent* Comp, int32 Index)
{
    if (!Actor) return;
 
    const FString ID = TryGetID(Actor);
 
    AActor* From = this;
    if (AActor* VehicleActorPtr = GetParentActor())
    {
        From = VehicleActorPtr;
    }
 
    if (Vehicle == Actor)
    {
        // BP_Drone is implemented in a way that the Actor itself doesn't move,
        // only its skeletal mesh component does. So ignore overlapping with the vehicle itself.
        return;
    }
 
    FVector Location = Actor->GetActorLocation();
 
    if (Comp)
    {
        if (const UInstancedStaticMeshComponent* ISM = Cast<UInstancedStaticMeshComponent>(Comp))
        {
            if (Index >= 0)
            {
                FTransform Xf;
                if (ISM->GetInstanceTransform(Index, Xf, true))
                {
                    // If ISM and valid index, use the instance's world transform for location
                    Location = Xf.GetLocation();
                }
            }
        }
    }
 
    // Distance from sensor parent (vehicle) to the (instance) location
    const float DistanceInMeters = FVector::Dist(From->GetActorLocation(), Location) / 100.0f;
 
    const FString Phase = Prefix.Replace(TEXT(" "), TEXT("_"));
    const FString ActorNm = Actor->GetName();
    const FString CompCls = Comp ? Comp->GetClass()->GetName() : TEXT("none");
 
    // Try to get the mesh name
    FString MeshName;
    if (const UInstancedStaticMeshComponent* ISM = Cast<UInstancedStaticMeshComponent>(Comp))
    {
        if (const UStaticMesh* SM = ISM->GetStaticMesh()) { MeshName = SM->GetName(); }
    }
    else if (const UStaticMeshComponent* SMC = Cast<UStaticMeshComponent>(Comp))
    {
        if (const UStaticMesh* SM = SMC->GetStaticMesh()) { MeshName = SM->GetName(); }
    }
    else if (const USkeletalMeshComponent* SKC = Cast<USkeletalMeshComponent>(Comp))
    {
        if (const USkeletalMesh* Skel = SKC->GetSkeletalMeshAsset()) { MeshName = Skel->GetName(); }
    }
 
    auto Quote = [](FString S)
        {
            S.ReplaceInline(TEXT("\""), TEXT("\\\"")); return FString::Printf(TEXT("%s"), *S);
        };
    const FString Timestamp = CreateTimeStampString();
 
    const FString FullMessage = FString::Printf(
        TEXT("phase=%s actor=%s id=%s distance_m=%.2f location=%.2f,%.2f,%.2f component=%s mesh=%s timestamp=%s"),
        *Phase,
        *Quote(ActorNm),
        *Quote(ID),
        DistanceInMeters,
        Location.X, Location.Y, Location.Z,
        *CompCls,
        *Quote(MeshName),
        *Quote(Timestamp));
 
    UTopic* Topic = GetROSTopic();
    if (ROSMessage.IsValid() && Topic && IsROSConnected())
    {
        ROSMessage->_Data = FullMessage;
        Topic->Publish(ROSMessage);
    }
 
    WriteToLogFile(FullMessage);
}
 
void AOverlapSensor::CreateDataSavePath()
{
    if (!FileSavePath.IsEmpty())
    {
        return;
    }
 
    const FString DataLocation = UAgrarsensePaths::GetDataFolder();
 
    // Default save path
    FileSavePath = DataLocation + GetActorID_Implementation() + "/";
 
    if (AVehicle* AttachedVehicle = IsAttachedToVehicle())
    {
        // If sensor is attached to vehicle, save data to vehicle folder
        FileSavePath = DataLocation + AttachedVehicle->GetActorID_Implementation();
    }
}
 
void AOverlapSensor::CreateLogFile()
{
    if (IsValid(LogFile))
    {
        // File has already been created, return
        return;
    }
 
    CreateDataSavePath();
 
    FLogFileSettings Settings;
    Settings.FileCreationOptions = FFileCreationOptions::Overwrite;
    Settings.FileWriteOptions = FFileWriteOptions::Queue;
    Settings.QueueLength = MAX_int32;
    Settings.KeepFileOpen = false;
    Settings.Timestamp = false;
    Settings.OverrideFilePath = true;
    Settings.FilePath = FileSavePath;
 
    UE_LOG(LogTemp, Warning, TEXT("FileSavePath %s"), *FileSavePath);
 
    LogFile = NewObject<ULogFile>(ULogFile::StaticClass());
    if (IsValid(LogFile))
    {
        FString Prefix;
        if (AVehicle* AttachedVehicle = IsAttachedToVehicle())
        {
            Prefix = AttachedVehicle->GetActorID_Implementation();
        }
        FString FileName = Prefix.IsEmpty() ? TEXT("overlap") : Prefix + TEXT("_overlap");
 
        LogFile->Create(FileName, Settings);
    }
}
 
FString AOverlapSensor::TryGetID(AActor* Actor)
{
    FString ID;
 
    if (Actor && Actor->GetClass()->ImplementsInterface(UActorInformation::StaticClass()))
    {
        if (Actor->Implements<UActorInformation>())
        {
            ID = IActorInformation::Execute_GetActorID(Actor);
        }
    }
 
    return ID;
}