TransformSensor.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 "TransformSensor.h"
 
#include "Agrarsense/Vehicle/Vehicle.h"
#include "Agrarsense/Vehicle/VehicleTypes.h"
#include "Agrarsense/Utils/CoordinateConversionUtilities.h"
 
#include "GeoReferencingSystem.h"
#include "Kismet/GameplayStatics.h"
#include "Engine/World.h"
 
ATransformSensor::ATransformSensor(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
    // TickManager handles this Actor ticking
    PrimaryActorTick.bCanEverTick = false;
}
 
void ATransformSensor::Init(FTransformSensorParameters Params, bool SimulateSensor)
{
    SetSimulateSensor(SimulateSensor);
    Parameters = Params;
 
    ROSMessages::geometry_msgs::Transform transformMsg;
    TransformMessage = MakeShared<ROSMessages::geometry_msgs::Transform>(transformMsg);
 
    GnssMessageString = MakeShared<ROSMessages::std_msgs::String>();
 
    NavSatFixMessage = MakeShared<ROSMessages::sensor_msgs::NavSatFix>();
    NavSatFixMessage->header.frame_id = "world";
    NavSatFixMessage->position_covariance = {
        1.0, 0,   0,
        0,   1.0, 0,
        0,   0,   1.0
    };
    NavSatFixMessage->position_covariance_type = NavSatFixMessage->CovarianceType::COVARIANCE_TYPE_DIAGONAL_KNOWN;
    NavSatFixMessage->status.status = NavSatFixMessage->status.STATUS_FIX;
    NavSatFixMessage->status.service = NavSatFixMessage->status.SERVICE_GPS;
 
    CreateROSTopic();
    CreateLogFile();
}
 
void ATransformSensor::BeginPlay()
{
    Super::BeginPlay();
 
    SetActorEnableCollision(false);
 
    TickEntry = ATickManager::AddTick(this, BindTick(this, &ATransformSensor::TickParallel), ETickType::LateTickParallel);
}
 
void ATransformSensor::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    ATickManager::RemoveTick(TickEntry);
 
    DestroyROSTopic();
 
    Parameters.Clear();
    TransformMessage.Reset();
    GnssMessageString.Reset();
    NavSatFixMessage.Reset();
 
    Super::EndPlay(EndPlayReason);
}
 
void ATransformSensor::CreateROSTopic()
{
    Super::CreateROSTopic();
 
    GeoReferencingSystem = AGeoReferencingSystem::GetGeoReferencingSystem(GetWorld());
 
    // If map has been georeferenced, create gnss and navsatfix topics.
    if (GeoReferencingSystem)
    {
        FString BaseTopicName = FString::Printf(TEXT("/agrarsense/out/sensors/%s"), *GetSensorIdentifier());
        FString GnssTopicName;
        FString NavSatFixTopicName;
 
        int32 LastSlashIndex = BaseTopicName.Find(TEXT("/"), ESearchCase::IgnoreCase, ESearchDir::FromEnd);
        if (LastSlashIndex != INDEX_NONE)
        {
            GnssTopicName = BaseTopicName.Left(LastSlashIndex) + TEXT("/gnss");
            NavSatFixTopicName = BaseTopicName.Left(LastSlashIndex) + TEXT("/navsatfix");
        }
        else
        {
            GnssTopicName = BaseTopicName + TEXT("/gnss");
            NavSatFixTopicName = BaseTopicName + TEXT("/navsatfix");
        }
 
        GnssTopic = NewObject<UTopic>(UTopic::StaticClass());
        if (GnssTopic)
        {
            GnssTopic->Init(ROSInstance->ROSIntegrationCore, GnssTopicName, TEXT("std_msgs/String"));
            GnssTopic->Advertise();
        }
 
        NavSatFixTopic = NewObject<UTopic>(UTopic::StaticClass());
        if (NavSatFixTopic)
        {
            NavSatFixTopic->Init(ROSInstance->ROSIntegrationCore, NavSatFixTopicName, TEXT("sensor_msgs/NavSatFix"));
            NavSatFixTopic->Advertise();
        }
    }
}
 
void ATransformSensor::DestroyROSTopic()
{
    if (GnssTopic)
    {
        GnssTopic->Unadvertise();
        GnssTopic->Unsubscribe();
        GnssTopic->MarkAsDisconnected();
        GnssTopic->ConditionalBeginDestroy();
        GnssTopic = nullptr;
    }
 
    if (NavSatFixTopic)
    {
        NavSatFixTopic->Unadvertise();
        NavSatFixTopic->Unsubscribe();
        NavSatFixTopic->MarkAsDisconnected();
        NavSatFixTopic->ConditionalBeginDestroy();
        NavSatFixTopic = nullptr;
    }
 
    if (ROSTopic)
    {
        ROSTopic->Unadvertise();
        ROSTopic->Unsubscribe();
        ROSTopic->MarkAsDisconnected();
        ROSTopic->ConditionalBeginDestroy();
        ROSTopic = nullptr;
    }
}
 
void ATransformSensor::TickParallel(float DeltaTime)
{
    if (!CanSimulateSensor())
    {
        return;
    }
 
    // Determine current transform
    FTransform CurrentTransform;
    const bool HasValidTransform =
        (Parameters.UseOwningActorTransform && Parameters.OwningActor && (CurrentTransform = Parameters.OwningActor->GetTransform(), true)) ||
        (Parameters.PrimitiveComponent && (CurrentTransform = Parameters.PrimitiveComponent->GetRelativeTransform(), true));
 
    if (!HasValidTransform)
    {
        return;
    }
 
    const FVector CurrentPosition = CurrentTransform.GetLocation();
    const FQuat CurrentRotation = CurrentTransform.GetRotation();
 
    const float PositionToleranceSq = 0.01f;
    const float RotationTolerance = 0.001f;
 
    const bool PositionChanged = FVector::DistSquared(CurrentPosition, PreviousPosition) > PositionToleranceSq;
    const bool RotationChanged = !CurrentRotation.Equals(PreviousRotation, RotationTolerance);
 
    if (PositionChanged || RotationChanged)
    {
        PreviousPosition = CurrentPosition;
        PreviousRotation = CurrentRotation;
 
        if (IsROSConnected()) 
        {
            SendTransformDataToROS(CurrentPosition, CurrentRotation);
            SendGpsDataToROS(CurrentPosition);
        }
        
        SaveTransformMetaDataToDisk(CurrentPosition, CurrentRotation);
    }
}
 
void ATransformSensor::SendTransformDataToROS(const FVector& Translation, const FQuat& Rotation)
{
    UTopic* Topic = GetROSTopic();
    if (Topic && TransformMessage.IsValid())
    {
        TransformMessage->translation = Translation;
        TransformMessage->rotation = Rotation;
 
        Topic->Publish(TransformMessage);
    }
}
 
void ATransformSensor::SendGpsDataToROS(const FVector& ActorPosition)
{
    if (!GeoReferencingSystem)
    {
        return;
    }
 
    // Convert Actor position to geographic coordinates
    FGeographicCoordinates GeographicCoordinates = UCoordinateConversionUtilities::UnrealToGeographicCoordinates(GeoReferencingSystem, ActorPosition);
    const double Latitude = GeographicCoordinates.Latitude;
    const double Longitude = GeographicCoordinates.Longitude;
    const double Altitude = GeographicCoordinates.Altitude;
 
    // gps data as string topic: Latitude, Longitude, Altitude
    if (GnssTopic && GnssMessageString.IsValid())
    {
        const FString GnssDataString = FString::Printf(TEXT("%f, %f, %f"), Latitude, Longitude, Altitude);
        GnssMessageString->_Data = GnssDataString;
        GnssTopic->Publish(GnssMessageString);
    }
 
    // gps data as NavSatFix ROS message
    if (NavSatFixTopic && NavSatFixMessage.IsValid())
    {
        NavSatFixMessage->latitude = Latitude;
        NavSatFixMessage->longitude = Longitude;
        NavSatFixMessage->altitude = Altitude;
        NavSatFixMessage->header.time = FROSTime::Now();
 
        NavSatFixTopic->Publish(NavSatFixMessage);
    }
}
 
void ATransformSensor::SaveTransformMetaDataToDisk(const FVector& ActorPosition, const FQuat& Rotation)
{
    if (!Parameters.SaveTransformDataToDisk || !IsValid(LogFile))
    {
        return;
    }
 
    const FRotator ActorRotation = Rotation.Rotator();
 
    FString MetaData;
 
    FString TimeStamp = CreateTimeStampString();
 
    if (GeoReferencingSystem)
    {
        FGeographicCoordinates GeographicCoordinates = UCoordinateConversionUtilities::UnrealToGeographicCoordinates(GeoReferencingSystem, ActorPosition);
        const double Latitude = GeographicCoordinates.Latitude;
        const double Longitude = GeographicCoordinates.Longitude;
        const double Altitude = GeographicCoordinates.Altitude;
 
        MetaData = FString::Printf(TEXT("%s, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.8f, %.8f, %.8f"), *TimeStamp, ActorPosition.X, ActorPosition.Y, ActorPosition.Z,
            ActorRotation.Pitch, ActorRotation.Yaw, ActorRotation.Roll,
            GeographicCoordinates.Latitude, GeographicCoordinates.Longitude, GeographicCoordinates.Altitude);
    }
    else
    {
        MetaData = FString::Printf(TEXT("%s, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f"), *TimeStamp, ActorPosition.X, ActorPosition.Y, ActorPosition.Z,
            ActorRotation.Pitch, ActorRotation.Yaw, ActorRotation.Roll);
    }
 
    // Write to the log file (this is written after sensor is destroyed)
    WriteToLogFile(MetaData);
}
 
void ATransformSensor::CreateLogFile()
{
    if (!Parameters.SaveTransformDataToDisk || IsValid(LogFile))
    {
        return;
    }
 
    CreateDataSavePath();
 
    FLogFileSettings Settings;
    Settings.FileCreationOptions = FFileCreationOptions::Overwrite;
    Settings.FileWriteOptions = FFileWriteOptions::Queue;
    Settings.QueueLength = MAX_int32; // Only write the log after destroying the sensor
    Settings.KeepFileOpen = false;
    Settings.Timestamp = false;
    Settings.OverrideFilePath = true;
    Settings.FilePath = FileSavePath;
 
    LogFile = NewObject<ULogFile>(ULogFile::StaticClass());
    if (IsValid(LogFile))
    {
        FString Prefix;
        if (AVehicle* Vehicle = IsAttachedToVehicle())
        {
            Prefix = Vehicle->GetActorID_Implementation();
        }
        FString FileName = Prefix.IsEmpty() ? TEXT("transform") : Prefix + TEXT("_transform");
 
        LogFile->Create(FileName, Settings);
 
        AGeoReferencingSystem* GeoRef = AGeoReferencingSystem::GetGeoReferencingSystem(GetWorld());
        if (GeoRef)
        {
            WriteToLogFile("timestamp, X location, Y location, Z location, yaw rotation, pitch rotation, roll rotation, GPS latitude, GPS longitude, GPS altitude");
        }
        else
        {
            WriteToLogFile("timestamp, X location, Y location, Z location, yaw rotation, pitch rotation, roll rotation");
        }
    }
}
 
void ATransformSensor::CreateDataSavePath()
{
    if (!FileSavePath.IsEmpty())
    {
        return;
    }
 
    const FString DataLocation = UAgrarsensePaths::GetDataFolder();
 
    // Default save path
    FileSavePath = DataLocation + GetActorID_Implementation() + "/"; 
 
    if (AVehicle* Vehicle = IsAttachedToVehicle())
    {
        // If sensor is attached to vehicle, save data to vehicle folder
        const FString VehicleName = Vehicle->GetActorID_Implementation();
        FileSavePath = DataLocation + VehicleName;
    }
    else if (ParentActor)
    {
        if (ASensor* ParentSensor = Cast<ASensor>(ParentActor))
        {
            if (AVehicle* SensorVehicle = ParentSensor->IsAttachedToVehicle())
            {
                // If this TransformSensor is attached to a sensor that's attached to a vehicle,
                // save data to Vehicle/Sensor folder
                const FString VehicleName = SensorVehicle->GetActorID_Implementation();
                const FString ParentSensorName = ParentSensor->GetActorID_Implementation();
                FileSavePath = DataLocation + VehicleName + "/" + ParentSensorName;
            }
        }
    }
}