Sensor.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 "Sensor.h"
#include "SensorManager.h"
#include "SemanticColors.h"
#include "SensorFactory.h"
 
#include "Agrarsense/Vehicle/Vehicle.h"
#include "Agrarsense/Sensor/Transform/TransformSensorParameters.h"
#include "Agrarsense/JSON/SimulatorJsonExporter.h"
#include "Agrarsense/Game/AgrarsenseStatics.h"
#include "Agrarsense/Logging/SimulatorLog.h"
#include "Agrarsense/ROS/ROSHandler.h"
#include "Agrarsense/ActorInterfaces/ActorInformation.h"
 
#include "Misc/DateTime.h"
#include "Kismet/GameplayStatics.h"
#include "TimerManager.h"
#include "UObject/Class.h"
 
TArray<TWeakObjectPtr<UPrimitiveComponent>> ASensor::ComponentsToHide;
FPrimitiveAdded ASensor::OnPrimitiveAdded;
 
ASensor::ASensor(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
    RootComponent = CreateDefaultSubobject<USceneComponent>(TEXT("SceneRoot"));
    RootComponent->SetMobility(EComponentMobility::Movable);
}
 
void ASensor::BeginPlay()
{
    Super::BeginPlay();
 
    UWorld* World = GetWorld();
 
    UROSHandler* ROSHandler = UAgrarsenseStatics::GetROSHandle(World);
    if (ROSHandler)
    {
        // Subscribe to ROS changed event
        ROSHandler->OnROSStateChanged.AddUniqueDynamic(this, &ASensor::ROSBridgeStateChanged);
    }
 
    ROSInstance = UAgrarsenseStatics::GetROSGameInstance(World);
    if (ROSInstance)
    {
        // Check if ROS is connected at BeginPlay
        ROSConnected = ROSInstance->IsROSConnected();
    }
 
    // Delay logging sensor information message so each sensor have time setup fully.
    FTimerHandle Handle;
    World->GetTimerManager().SetTimer(Handle, FTimerDelegate::CreateLambda([this]
        {
            if (IsValid(this))
            {
                FString ActorInformation = IActorInformation::Execute_GetActorInformation(this);
                FString Message = FString::Printf(TEXT("Spawned %s"), *ActorInformation);
 
                SimulatorLog::Log(Message);
            }
        }), 0.150f, false);
 
    // Inform sensor manager that new sensor has been spawned.
    USensorManager::AddSensor(this);
 
    ESensorTypes ThisSensorType = GetSensorType();
 
    if (ThisSensorType != ESensorTypes::NONE &&
        ThisSensorType != ESensorTypes::Transform &&
        ThisSensorType != ESensorTypes::Collision &&
        ThisSensorType != ESensorTypes::Overlap)
    {
        // Spawn a Transform sensor for any sensor type other than NONE, Transform, Collision, or Overlap.
        // Meaning each sensor, like a Lidar sensor, has its own Transform sensor that publishes its:
        // - World location
        // - GNSS location (if world has GNSS setup)
        
        // We don't need to keep track of this TransformSensor Actor as it should 
        // be automatically destroyed when the parent sensor is destroyed (ASensor EndPlay)
        const FString ActorID = IActorInformation::Execute_GetActorID(this);
        const FString SensorTransformSensorID = ActorID + "/transform";
 
        FSensorSpawnParameters SpawnParams;
        SpawnParams.Transform = GetActorTransform();
        SpawnParams.SensorIdentifier = SensorTransformSensorID;
        SpawnParams.SensorName = "transform";
        SpawnParams.SimulateSensor = true;
        SpawnParams.Parent = this;
 
        FTransformSensorParameters SensorParams;
        SensorParams.SaveTransformDataToDisk = false;
        SensorParams.OwningActor = this;
 
        USensorFactory::SpawnTransformSensor(SpawnParams, SensorParams);
    }
}
 
void ASensor::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    // Broadcast that this sensor will be destroyed
    OnSensorDestroy.Broadcast(this);
 
    USensorManager::RemoveSensor(this);
 
    Super::EndPlay(EndPlayReason);
 
    UROSHandler* ROSHandler = UAgrarsenseStatics::GetROSHandle(GetWorld());
    if (ROSHandler)
    {
        ROSHandler->OnROSStateChanged.RemoveDynamic(this, &ASensor::ROSBridgeStateChanged);
    }
 
    const FString Sensor = UEnum::GetDisplayValueAsText(GetSensorType()).ToString();
    const FString ID = GetSensorIdentifier();
    const FString Message = FString::Printf(TEXT("Destroyed %s sensor with ID: %s"), *Sensor, *ID);
    SimulatorLog::Log(Message);
 
    if (EndPlayReason == EEndPlayReason::EndPlayInEditor 
        || EndPlayReason == EEndPlayReason::Quit)
    {
        for (auto& WeakComponentPtr : ComponentsToHide)
        {
            WeakComponentPtr.Reset();
        }
        ComponentsToHide.Empty();
    }
 
    // Destroy all Actors that are attached to this sensor like:
    // Sensor model Actor and TransformSensor Actor.
    TArray<AActor*> AttachedActors;
    GetAttachedActors(AttachedActors);
    for (AActor* Actor : AttachedActors)
    {
        if (Actor)
        {
            Actor->Destroy();
        }
    }
 
    ROSInstance = nullptr;
 
    if (LogFile)
    {
        LogFile->Destroy();
        LogFile = nullptr;
    }
 
    DestroyROSTopic();
}
 
FString ASensor::ExportToJsonFile(const FString& FileName)
{
    return USimulatorJsonExporter::ExportSensorToJSON(FileName, this);
}
 
AVehicle* ASensor::IsAttachedToVehicle() const
{
    AVehicle* VehicleActor = Cast<AVehicle>(ParentActor);
    if (VehicleActor)
    {
        return VehicleActor;
    }
    return nullptr;
}
 
void ASensor::ROSBridgeStateChanged(EROSState ROSState)
{
    switch (ROSState)
    {
    case EROSState::Connected:
        ROSConnected = true;
        CreateROSTopic();
        break;
 
    case EROSState::Disconnected:
        ROSConnected = false;
        DestroyROSTopic();
        break;
    }
}
 
void ASensor::DestroyROSTopic()
{
    if (ROSTopic)
    {
        ROSTopic->Unadvertise();
        ROSTopic->Unsubscribe();
        ROSTopic->MarkAsDisconnected();
        ROSTopic->ConditionalBeginDestroy();
        ROSTopic = nullptr;
    }
}
 
void ASensor::CreateROSTopic()
{
    if (!SendDataToROS
        || ROSTopic
        || !ROSInstance
        || !IsROSConnected())
    {
        return;
    }
 
    FString MessageType;
    ESensorTypes SensorType = GetSensorType();
 
    switch (SensorType)
    {
    case ESensorTypes::Lidar:
    case ESensorTypes::Radar:
        MessageType = "sensor_msgs/PointCloud2";
        break;
 
    case ESensorTypes::RGBCamera:
    case ESensorTypes::ThermalCamera:
    case ESensorTypes::DepthCamera:
    case ESensorTypes::SemanticSegmentationCamera:
    case ESensorTypes::InstanceSegmentationCamera:
    case ESensorTypes::DVSCamera: // DVS Camera should override this.
        MessageType = "sensor_msgs/Image";
        break;
 
    case ESensorTypes::Transform:
        MessageType = "geometry_msgs/Transform";
        break;
 
    case ESensorTypes::Collision:
    case ESensorTypes::Overlap:
        MessageType = "std_msgs/String";
        break;
 
    default:
        FString SensorNameString = UEnumUtilities::ConvertSensorTypeToString(SensorType);
        UE_LOG(LogTemp, Warning, TEXT("Unhandled ROS sensor message creation for sensor: %s"), *SensorNameString);
        break;
    }
 
    const FString TopicName = FString::Printf(TEXT("/agrarsense/out/sensors/%s"), *GetSensorIdentifier());
 
    ROSTopic = NewObject<UTopic>(UTopic::StaticClass());
    if (ROSTopic)
    {
        ROSTopic->Init(ROSInstance->ROSIntegrationCore, TopicName, MessageType);
        ROSTopic->Advertise();
    }
 
    const FString Message = FString::Printf(TEXT("%s sensor created ROS topic. Name: '%s' MessageType: '%s' "), *GetSensorName(), *TopicName, *MessageType);
    SimulatorLog::Log(Message);
}
 
void ASensor::CreateDataSavePath()
{
    if (FileSavePath.IsEmpty())
    {
        const FString DataLocation = UAgrarsensePaths::GetDataFolder();
    
        if (AVehicle* Vehicle = IsAttachedToVehicle()) 
        {
            FString VehicleName = Vehicle->GetActorID_Implementation();
            FileSavePath = DataLocation + VehicleName + "/" + GetActorID_Implementation() + "/";
        }
        else 
        {
            FileSavePath = DataLocation + GetActorID_Implementation() + "/";
        }
    }
}
 
void ASensor::CreateLogFile()
{
    if (IsValid(LogFile))
    {
        // File has already been created, return
        return;
    }
 
    FLogFileSettings Settings;
    Settings.FileCreationOptions = FFileCreationOptions::Overwrite;
    Settings.FileWriteOptions = FFileWriteOptions::Queue;
    Settings.QueueLength = 10;
    Settings.KeepFileOpen = false;
 
    LogFile = NewObject<ULogFile>(ULogFile::StaticClass());
    if (LogFile)
    {
        // FileName is SensorName_ID_UnixTimeStamp.txt
        FString UnixTimeStamp = FString::FromInt(FDateTime::Now().ToUnixTimestamp());
 
        FString ActorID = IActorInformation::Execute_GetActorID(this);
 
        // Temp fix, Remove all "/" from ActorID
        ActorID.ReplaceInline(TEXT("/"), TEXT(""), ESearchCase::IgnoreCase);
 
        FString FileName = FString::Printf(TEXT("%s_%s_%s"), *GetSensorName(), *ActorID, *UnixTimeStamp);
        LogFile->Create(FileName, Settings);
    }
}
 
void ASensor::WriteToLogFile(const FString& Message)
{
    if (!IsValid(LogFile) && IsValid(this))
    {
        CreateLogFile();
    }
 
    if (IsValid(LogFile))
    {
        LogFile->Write(Message);
    }
}
 
void ASensor::HideComponentForAllCameras(UPrimitiveComponent* PrimitiveComponent)
{
    if (PrimitiveComponent)
    {
        TWeakObjectPtr<UPrimitiveComponent> Component(PrimitiveComponent);
        ComponentsToHide.Add(Component);
        OnPrimitiveAdded.Broadcast(PrimitiveComponent);
    }
}
 
TMap<FString, FColor> ASensor::GetSemanticColors()
{
    return USemanticColors::GetSemanticColors();
}
 
FString ASensor::CreateTimeStampString() const
{
    FDateTime Now = FDateTime::Now();
    FString TimeStamp = FString::Printf(TEXT("%02d:%02d:%04d:%02d:%02d:%02d:%03d"),
        Now.GetDay(), Now.GetMonth(), Now.GetYear(),
        Now.GetHour(), Now.GetMinute(), Now.GetSecond(), Now.GetMillisecond());
 
    return TimeStamp;
}