CollisionSensor.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 "CollisionSensor.h"
#include "Collision.h"
 
#include "Agrarsense/Vehicle/Vehicle.h"
#include "Agrarsense/Logging/SimulatorLog.h"
 
ACollisionSensor::ACollisionSensor(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
    PrimaryActorTick.bCanEverTick = false;
}
 
void ACollisionSensor::Init(FCollisionSensorParameters CollisionSensorParameters, bool SimulateSensor)
{
    SetSimulateSensor(SimulateSensor);
 
    if (CollisionSensorParameters.OwningActor)
    {
        CurrentOwner = CollisionSensorParameters.OwningActor;
        PrimitiveComponent = CollisionSensorParameters.PrimitiveComponent;
 
        if (CollisionSensorParameters.UseActorCollision && CurrentOwner.Get())
        {
            CurrentOwner->OnActorHit.AddDynamic(this, &ACollisionSensor::OnActorHit);
        }
        else if (CollisionSensorParameters.PrimitiveComponent) 
        {
            PrimitiveComponent->OnComponentHit.AddDynamic(this, &ACollisionSensor::OnComponentHit);
        }
 
        AttachToActor(CurrentOwner.Get(), FAttachmentTransformRules::KeepWorldTransform);
    }
 
    CollisionMessage = MakeShared<ROSMessages::std_msgs::String>();
 
    CreateROSTopic();
    CreateLogFile();
}
 
void ACollisionSensor::BeginPlay()
{
    Super::BeginPlay();
}
 
void ACollisionSensor::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
 
    if (CurrentOwner.Get())
    {
        CurrentOwner->OnActorHit.RemoveDynamic(this, &ACollisionSensor::OnActorHit);
        CurrentOwner.Reset();
    }
 
    if (PrimitiveComponent)
    {
        PrimitiveComponent->OnComponentHit.RemoveDynamic(this, &ACollisionSensor::OnComponentHit);
        PrimitiveComponent = nullptr;
    }
 
    Parameters.Reset();
    CollisionMessage.Reset();
}
 
void ACollisionSensor::OnComponentHit(UPrimitiveComponent* HitComponent, AActor* OtherActor, UPrimitiveComponent* OtherComp, FVector NormalImpulse, const FHitResult& Hit)
{
    if (!CanSimulateSensor() || !OtherActor || !CurrentOwner.Get())
    {
        return;
    }
 
    NormalImpulse *= 1e-2;
    if (NormalImpulse.Size() > 0.3f)
    {
        CollisionData Collisiondata =
        {
            CurrentOwner.Get(),
            OtherActor,
            CurrentOwner->GetName(),
            OtherActor->GetName(),
            NormalImpulse
        };
 
        FString CollisionDataString = CollsionDataToString(Collisiondata);
        SendCollisionData(CollisionDataString);
        WriteToLogFile(CollisionDataString);
    }
}
 
void ACollisionSensor::OnActorHit(AActor* Actor, AActor* OtherActor, FVector NormalImpulse, const FHitResult& Hit)
{
    if (!CanSimulateSensor() || !Actor || !OtherActor)
    {
        return;
    }
 
    NormalImpulse *= 1e-2;
    if (NormalImpulse.Size() > 200)
    {
        CollisionData Collisiondata =
        {
            Actor,
            OtherActor,
            Actor->GetName(),
            OtherActor->GetName(),
            NormalImpulse
        };
 
        FString CollisionDataString = CollsionDataToString(Collisiondata);
        SendCollisionData(CollisionDataString);
        WriteToLogFile(CollisionDataString);
    }
}
 
FString ACollisionSensor::CollsionDataToString(const CollisionData& Data)
{
    auto Quote = [](FString S)
        {
            S.ReplaceInline(TEXT("\""), TEXT("\\\""));
            return FString::Printf(TEXT("%s"), *S);
        };
 
 
    FString ID;
    FVector Location;
 
    if (Data.OtherActorPtr)
    {
        ID = TryGetID(Data.OtherActorPtr);
        Location = Data.ActorPtr->GetActorLocation();
    }
 
    auto NormalImpulse = Data.NormalImpulse;
    const FString ImpulseStr = FString::Printf(TEXT("%.2f,%.2f,%.2f"), NormalImpulse.X, NormalImpulse.Y, NormalImpulse.Z);
    const double ImpulseMag = Data.NormalImpulse.Size();
    const FString Ts = CreateTimeStampString();
 
    return FString::Printf(
        TEXT("actor=%s other_actor=%s other_actor_id=%s other_actor_location=%.2f,%.2f,%.2f impulse_strength=%s impulse_magnitude=%.2f timestamp=%s"),
        *Quote(Data.Actor),
        *Quote(Data.OtherActor),
        *Quote(ID),
        Location.X, Location.Y, Location.Z,
        *ImpulseStr,
        ImpulseMag,
        *Quote(Ts));
}
 
FString ACollisionSensor::TryGetID(AActor* Actor) const
{
    FString ID;
 
    if (Actor && Actor->GetClass()->ImplementsInterface(UActorInformation::StaticClass()))
    {
        if (Actor->Implements<UActorInformation>())
        {
            ID = IActorInformation::Execute_GetActorID(Actor);
        }
    }
 
    return ID;
}
 
void ACollisionSensor::SendCollisionData(const FString& CollisionDataString)
{
    UTopic* Topic = GetROSTopic();
    if (IsROSConnected() && CollisionMessage.IsValid() && Topic)
    {
        CollisionMessage->_Data = CollisionDataString;
        Topic->Publish(CollisionMessage);
    }
}
 
void ACollisionSensor::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 ACollisionSensor::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("collision") : Prefix + TEXT("_collision");
 
        LogFile->Create(FileName, Settings);
    }
}