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/Game/AgrarsenseStatics.h"
#include "Agrarsense/ActorInterfaces/ActorInformation.h"
#include "Agrarsense/Logging/SimulatorLog.h"
 
#if WITH_EDITOR
#include "DrawDebugHelpers.h"
#endif
 
static constexpr float SIZE_DIVISOR = 70.0f;
 
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 you change DefaultEngine.ini collision channels then this index must be changed as well!
    const int32 OverlapSensorIndex = 2;
    const ECollisionChannel OverlapSensorChannel = UCollisionProfile::Get()->ConvertToCollisionChannel(false, OverlapSensorIndex);
    BoundsComponent->SetCollisionEnabled(ECollisionEnabled::QueryOnly);
    BoundsComponent->SetGenerateOverlapEvents(true);
    BoundsComponent->SetCollisionObjectType(OverlapSensorChannel);
    BoundsComponent->SetCollisionResponseToAllChannels(ECollisionResponse::ECR_Ignore);
    BoundsComponent->SetCollisionResponseToChannel(OverlapSensorChannel, ECollisionResponse::ECR_Overlap);
    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)
    {
        // Adjust the scale of the Sphere/Box component bounds to match the scale of the VisualizerComponent mesh
        FVector ScaledVector = FVector(Parameters.Size.X / SIZE_DIVISOR, Parameters.Size.Y / SIZE_DIVISOR, Parameters.Size.Z / SIZE_DIVISOR);
        BoundsComponent->SetRelativeScale3D(ScaledVector);
        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)
    {
        // Adjust the scale of the Sphere/Box component bounds to match the scale of the VisualizerComponent mesh
        FVector ScaledVector = FVector(Parameters.Size.X / SIZE_DIVISOR, Parameters.Size.Y / SIZE_DIVISOR, Parameters.Size.Z / SIZE_DIVISOR);
        BoundsComponent->SetRelativeScale3D(ScaledVector);
    }
 
    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)
{
    // Don't allow Overlapping with other Overlapping sensors..
    AOverlapSensor* OtherOverlapSensor = Cast<AOverlapSensor>(OtherActor);
    if (OtherOverlapSensor)
    {
        return;
    }
 
    BuildAndSendMessage("Overlap Begin with", OtherActor);
}
 
void AOverlapSensor::OnOverlapEnd(UPrimitiveComponent* OverlappedComponent, AActor* OtherActor,
    UPrimitiveComponent* OtherComp, int32 OtherBodyIndex)
{
    // Don't allow Overlapping with other Overlapping sensors..
    AOverlapSensor* OtherOverlapSensor = Cast< AOverlapSensor>(OtherActor);
    if (OtherOverlapSensor)
    {
        return;
    }
 
    BuildAndSendMessage("Overlap End with", OtherActor);
}
 
void AOverlapSensor::BuildAndSendMessage(const FString& Prefix, AActor* Actor)
{
    if (!Actor)
    {
        return;
    }
 
    FString ID = TryGetID(Actor);
    FString Message;
 
    // Calculate distance from sensor parent (vehicle) to incoming Actor
    AActor* From = this;
    if (AActor* VehicleActorPtr = GetParentActor())
    {
        From = VehicleActorPtr;
    }
    float DistanceInMeters = FVector::Dist(From->GetActorLocation(), Actor->GetActorLocation()) / 100.0f;
 
    // Add distance to the end of the message
    if (ID.IsEmpty())
    {
        Message = FString::Printf(TEXT("%s: %s, Distance: %.2f m"), *Prefix, *Actor->GetName(), DistanceInMeters);
    }
    else
    {
        Message = FString::Printf(TEXT("%s: %s: ID: %s, Distance: %.2f m"), *Prefix, *Actor->GetName(), *ID, DistanceInMeters);
    }
 
    UTopic* Topic = GetROSTopic();
    if (ROSMessage.IsValid() && Topic && IsROSConnected())
    {
        ROSMessage->_Data = Message;
        Topic->Publish(ROSMessage);
    }
 
    WriteToLogFile(Message);
 
    if (Vehicle)
    {
        FString Msg = FString::Printf(TEXT("Actor '%s' with ID: '%s' %s"), *VehicleName, *VehicleID, *Message);
        SimulatorLog::Log(Msg);
    }
}
 
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;
}