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;
 
    VisualizerComponent = CreateDefaultSubobject<UBoundsVisualizerComponent>(TEXT("BoundsVisualizerComponent"));
    VisualizerComponent->SetupAttachment(RootComponent);
    VisualizerComponent->SetGenerateOverlapEvents(false);
    VisualizerComponent->SetVisibility(false);
 
    BoxComponent = CreateDefaultSubobject<UBoxComponent>(TEXT("BoxComponent"));
    BoxComponent->SetGenerateOverlapEvents(false);
    BoxComponent->SetVisibility(false);
    
    SphereComponent = CreateDefaultSubobject<USphereComponent>(TEXT("SphereComponent"));
    SphereComponent->SetGenerateOverlapEvents(false);
    SphereComponent->SetVisibility(false);
}
 
void AOverlapSensor::Init(FOverlapSensorParameters InParameters)
{
    Parameters = InParameters;
 
    Vehicle = Cast<AVehicle>(Parameters.OwningActor);
 
    if (Vehicle) 
    {
        VehicleName = Vehicle->GetActorName_Implementation();
        VehicleID = Vehicle->GetActorID_Implementation();
    }
 
    OverlapShape = Parameters.TriggerShape;
 
    UPrimitiveComponent* TriggerComponent = (OverlapShape == ETriggerShape::Box)
        ? Cast<UPrimitiveComponent>(BoxComponent) : Cast<UPrimitiveComponent>(SphereComponent);
 
    // if you change DefaultEngine.ini collision channels then this index must be changed as well!
    const int32 OverlapSensorIndex = 2;
 
    if (OverlapShape == ETriggerShape::Box)
    {
        BoxComponent->SetGenerateOverlapEvents(true);
        SphereComponent->SetGenerateOverlapEvents(false);
        TriggerComponent = BoxComponent;
 
        const ECollisionChannel OverlapSensorChannel = UCollisionProfile::Get()->ConvertToCollisionChannel(false, OverlapSensorIndex);
        BoxComponent->SetCollisionObjectType(OverlapSensorChannel);
        BoxComponent->SetCollisionResponseToAllChannels(ECollisionResponse::ECR_Ignore);
        BoxComponent->SetCollisionResponseToChannel(OverlapSensorChannel, ECollisionResponse::ECR_Overlap);
        BoxComponent->SetRelativeLocation(Parameters.RelativePosition);
 
        SphereComponent->SetCollisionResponseToAllChannels(ECollisionResponse::ECR_Ignore);
 
        if (RootComponent) 
        {
            BoxComponent->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepWorldTransform);
        }
        
        if (VisualizerComponent)
        {
            VisualizerComponent->SetCubeMesh();
        }
    }
    else if (OverlapShape == ETriggerShape::Sphere)
    {
        BoxComponent->SetGenerateOverlapEvents(false);
        SphereComponent->SetGenerateOverlapEvents(true);
 
        const ECollisionChannel OverlapSensorChannel = UCollisionProfile::Get()->ConvertToCollisionChannel(false, OverlapSensorIndex);
        SphereComponent->SetCollisionObjectType(OverlapSensorChannel);
        SphereComponent->SetCollisionResponseToAllChannels(ECollisionResponse::ECR_Ignore);     
        SphereComponent->SetCollisionResponseToChannel(OverlapSensorChannel, ECollisionResponse::ECR_Overlap);
        SphereComponent->SetRelativeLocation(Parameters.RelativePosition);
 
        BoxComponent->SetCollisionResponseToAllChannels(ECollisionResponse::ECR_Ignore);
 
        if (RootComponent)
        {
            SphereComponent->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepWorldTransform);
        }
 
        TriggerComponent = SphereComponent;
    }
 
    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 (VisualizerComponent)
    {
        // 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);
        VisualizerComponent->SetRelativeScale3D(ScaledVector);
        VisualizerComponent->SetRelativeLocation(Parameters.RelativePosition);
 
        if (RootComponent)
        {
            VisualizerComponent->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepWorldTransform);
        }
    }
 
    if (Parameters.OwningActor)
    {
        AttachToActor(Parameters.OwningActor, FAttachmentTransformRules::KeepWorldTransform);
    }
}
 
void AOverlapSensor::BeginPlay()
{
    Super::BeginPlay();
 
    if (OverlapShape == ETriggerShape::Box)
    {
        BoxComponent->OnComponentBeginOverlap.AddDynamic(this, &AOverlapSensor::OnOverlapBegin);
        BoxComponent->OnComponentEndOverlap.AddDynamic(this, &AOverlapSensor::OnOverlapEnd);
    }
    else if (OverlapShape == ETriggerShape::Sphere)
    {
        SphereComponent->OnComponentBeginOverlap.AddDynamic(this, &AOverlapSensor::OnOverlapBegin);
        SphereComponent->OnComponentEndOverlap.AddDynamic(this, &AOverlapSensor::OnOverlapEnd);
    }
}
 
void AOverlapSensor::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
 
    if (OverlapShape == ETriggerShape::Box)
    {
        BoxComponent->OnComponentBeginOverlap.RemoveDynamic(this, &AOverlapSensor::OnOverlapBegin);
        BoxComponent->OnComponentEndOverlap.RemoveDynamic(this, &AOverlapSensor::OnOverlapEnd);
    }
    else if (OverlapShape == ETriggerShape::Sphere)
    {
        SphereComponent->OnComponentBeginOverlap.RemoveDynamic(this, &AOverlapSensor::OnOverlapBegin);
        SphereComponent->OnComponentEndOverlap.RemoveDynamic(this, &AOverlapSensor::OnOverlapEnd);
    }
}
 
void AOverlapSensor::SetOverlapBounds(const FVector& NewSize)
{
    Parameters.Size = NewSize;
 
    if (OverlapShape == ETriggerShape::Box && BoxComponent)
    {
        BoxComponent->SetBoxExtent(NewSize);
    }
    else if (OverlapShape == ETriggerShape::Sphere && SphereComponent)
    {
        SphereComponent->SetSphereRadius(FMath::Max3(NewSize.X, NewSize.Y, NewSize.Z));
    }
 
    if (VisualizerComponent)
    {
        // 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);
        VisualizerComponent->SetRelativeScale3D(ScaledVector);
    }
 
    FString Msg = FString::Printf(TEXT("OverlapSensor with ID: %s changed overlap bounds to: %s"), *GetActorID_Implementation(), *NewSize.ToString());
    SimulatorLog::Log(Msg);
}
 
void AOverlapSensor::SetVisualizeOverlapArea(bool Visualize)
{
    IsVisible = Visualize;
 
    if (VisualizerComponent)
    {
        VisualizerComponent->SetVisibility(Visualize);
    }
 
#if WITH_EDITOR
    // Visualize BoxComponent/SphereComponent with DrawDebug for debugging purposes,
    // BoxComponent/SphereComponent and VisualizerComponent bounds should be equal
    if (Visualize)
    {
        if (OverlapShape == ETriggerShape::Box && BoxComponent)
        {
            BoxComponent->SetGenerateOverlapEvents(true);
            DrawDebugBox(GetWorld(), GetActorLocation(), BoxComponent->GetScaledBoxExtent(), FColor::Red, false, 3.0f);
        }
        else if (OverlapShape == ETriggerShape::Sphere && SphereComponent)
        {
            SphereComponent->SetGenerateOverlapEvents(true);
            DrawDebugSphere(GetWorld(), GetActorLocation(), SphereComponent->GetScaledSphereRadius(), 32, FColor::Red, false, 3.0f);
        }
    }
 
    UE_LOG(LogTemp, Warning, TEXT("OverlapSensor.cpp: Visualize overlap bounds: %s"), (Visualize ? TEXT("true") : TEXT("false")));
#endif
}
 
void AOverlapSensor::SetOverlapRelativePosition(const FVector& Vector)
{
    Parameters.RelativePosition = Vector;
 
    if (SphereComponent) 
    {
        SphereComponent->SetRelativeLocation(Parameters.RelativePosition);
    }
 
    if (BoxComponent) 
    {
        BoxComponent->SetRelativeLocation(Parameters.RelativePosition);
    }
 
    if (VisualizerComponent) 
    {
        VisualizerComponent->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 (SphereComponent)
    {
        SphereComponent->SetCollisionResponseToAllChannels(Response);
    }
 
    if (BoxComponent)
    {
        BoxComponent->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;
 
    if (ID.IsEmpty())
    {
        Message = FString::Printf(TEXT("%s: %s"), *Prefix, *Actor->GetName());
    }
    else
    {
        Message = FString::Printf(TEXT("%s: %s: ID: %s"), *Prefix, *Actor->GetName(), *ID);
    }
 
    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);
    }
}
 
FString AOverlapSensor::TryGetID(AActor* Actor)
{
    FString ID;
 
    if (Actor && Actor->GetClass()->ImplementsInterface(UActorInformation::StaticClass()))
    {
        IActorInformation* ActorInfo = Cast<IActorInformation>(Actor);
        if (ActorInfo)
        {
            ID = ActorInfo->GetActorID_Implementation();
        }
    }
 
    return ID;
}