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(AActor* NewOwner, bool SimulateSensor)
{
    SetSimulateSensor(SimulateSensor);
 
    if (NewOwner)
    {
        NewOwner->OnActorHit.AddDynamic(this, &ACollisionSensor::OnCollisionEvent);
        CurrentOwner = NewOwner;
    }
    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.IsValid())
    {
        CurrentOwner->OnActorHit.RemoveDynamic(this, &ACollisionSensor::OnCollisionEvent);
    }
 
    CurrentOwner.Reset();
    CollisionMessage.Reset();
}
 
void ACollisionSensor::OnCollisionEvent(AActor* Actor, AActor* OtherActor, FVector NormalImpulse, const FHitResult& Hit)
{
    if (!CanSimulateSensor() || !Actor || !OtherActor)
    {
        return;
    }
 
    NormalImpulse *= 1e-2;
    if (NormalImpulse.Size() > 200)
    {
        CollisionData Collisiondata =
        {
            Actor->GetName(),
            OtherActor->GetName(),
            NormalImpulse
        };
 
        SendCollisionData(Collisiondata.ToString());
 
        FString MsgHumanReadable = Collisiondata.ToStringHumanReadable();
        WriteToLogFile(MsgHumanReadable);       // Log to separate file
        SimulatorLog::Log(MsgHumanReadable);    // Log to ROS info topic and Simulator log file
    }
}
 
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);
    }
}