#include <TransformSensor.h>

Inheritance diagram for ATransformSensor:

[legend]

Collaboration diagram for ATransformSensor:

[legend]

Public Member Functions
	ATransformSensor (const FObjectInitializer &ObjectInitializer)

void	Init (FTransformSensorParameters Params, bool SimulateSensor=true)

virtual ESensorTypes	GetSensorType () const override

Public Member Functions inherited from ASensor
	ASensor (const FObjectInitializer &ObjectInitializer)

FString	ExportToJsonFile (const FString &FileName)

virtual ESensorTypes	GetSensorType () const

FString	GetSensorIdentifier () const

void	SetSensorIdentifier (const FString newIdentifier)

FString	GetSensorName () const

virtual FString	GetParametersAsString () const

AVehicle *	IsAttachedToVehicle () const

void	SetSensorName (const FString newName)

FString	GetTopicName ()

UTopic *	GetROSTopic () const

void	SetSimulateSensor (bool SimulateSensor)

bool	CanSimulateSensor () const

ASensorModel *	GetSensorModel () const

void	SetSensorModel (ASensorModel *NewSensorModel)

FORCEINLINE bool	IsROSConnected () const

UROSIntegrationGameInstance *	GetROSGameInstance () const

virtual FString	GetActorID_Implementation () const override

virtual FString	GetActorName_Implementation () const override

virtual FString	GetActorInformation_Implementation () const override

virtual void	SetActorName_Implementation (const FString &NewActorName) override

virtual void	SetActorIDAndName_Implementation (const FString &NewActorName, const FString &NewID) override

void	SetParentActorPtr (AActor *ParentActorPtr)

Public Member Functions inherited from IActorInformation
FString	GetActorID () const

FString	GetActorName () const

FString	GetActorInformation () const

void	SetActorName (const FString &NewActorName)

void	SetActorIDAndName (const FString &NewActorName, const FString &NewID)

Protected Member Functions
virtual void	BeginPlay () override

virtual void	EndPlay (const EEndPlayReason::Type EndPlayReason) override

virtual void	CreateROSTopic () override

virtual void	DestroyROSTopic () override

void	CreateDataSavePath () override

Protected Member Functions inherited from ASensor
virtual void	BeginPlay () override

virtual void	EndPlay (const EEndPlayReason::Type EndPlayReason) override

FString	CreateTimeStampString () const

virtual void	CreateROSTopic ()

virtual void	DestroyROSTopic ()

virtual void	CreateDataSavePath ()

bool	IsLogFileCreated ()

virtual void	CreateLogFile ()

void	WriteToLogFile (const FString &Message)

Protected Attributes
FTransformSensorParameters	Parameters

Protected Attributes inherited from ASensor
UTopic *	ROSTopic = nullptr

bool	SendDataToROS = true

ULogFile *	LogFile = nullptr

AActor *	ParentActor = nullptr

FString	FileSavePath

UROSIntegrationGameInstance *	ROSInstance = nullptr

Private Member Functions
void	TickParallel (float DeltaTime)

void	SendTransformDataToROS (const FVector &Translation, const FQuat &Rotation)

void	SendGpsDataToROS (const FVector &ActorPosition)

void	SaveTransformMetaDataToDisk (const FVector &Translation, const FQuat &Rotation)

void	CreateLogFile () override

Private Attributes
TSharedPtr< ROSMessages::geometry_msgs::Transform >	TransformMessage

TSharedPtr< ROSMessages::sensor_msgs::NavSatFix >	NavSatFixMessage

FVector	PreviousPosition = FVector(0.0f, 0.0f, 0.0f)

FQuat	PreviousRotation = FQuat(0.0f, 0.0f, 0.0f, 0.0f)

FTickEntry	TickEntry

UTopic *	NavSatFixTopic = nullptr

AGeoReferencingSystem *	GeoReferencingSystem = nullptr

float	TimeSinceLastPublish = 0.0f

Additional Inherited Members
Static Public Member Functions inherited from ASensor
static void	HideComponentForAllCameras (UPrimitiveComponent *PrimitiveComponent)

static TMap< FString, FColor >	GetSemanticColors ()

static TArray< TWeakObjectPtr< UPrimitiveComponent > >	GetComponentsToHide ()

Static Public Member Functions inherited from IActorInformation
static void	SetAndValidateActorIDAndName (FString &ActorName, FString &ActorID, TWeakObjectPtr< AActor > Actor)

static bool	DestroyActorByID (const FString &ID)

static AActor *	GetActorByID (const FString &ID)

template<typename T >
static TArray< T * >	GetActorsWithInterface ()

static void	PrintAllIds ()

Public Attributes inherited from ASensor
FSensorDestroy	OnSensorDestroy

FString	AttachedToComponent

FName	AttachedToBone

Static Protected Member Functions inherited from ASensor
template<typename InStructType >
static FString	StructToString (const InStructType &InStruct)

Static Protected Attributes inherited from ASensor
static FPrimitiveAdded	OnPrimitiveAdded

static const FName	NiagaraPointsInt = "User.PointCount"

static const FName	NiagaraHitPoints = "User.HitPoints"

static const FName	NiagaraHitColors = "User.HitColors"

static const FName	NiagaraPointsFloat = "User.Test"

Detailed Description

Definition at line 26 of file TransformSensor.h.

Constructor & Destructor Documentation

◆ ATransformSensor()

ATransformSensor::ATransformSensor ( const FObjectInitializer & ObjectInitializer )

Definition at line 16 of file TransformSensor.cpp.

                                                                              : Super(ObjectInitializer)
{
    // TickManager handles this Actor ticking
    PrimaryActorTick.bCanEverTick = false;
}

Member Function Documentation

◆ BeginPlay()

void ATransformSensor::BeginPlay ( )

overrideprotectedvirtual

Reimplemented from ASensor.

Definition at line 47 of file TransformSensor.cpp.

{
    Super::BeginPlay();
 
    SetActorEnableCollision(false);
 
    TickEntry = ATickManager::AddTick(this, BindTick(this, &ATransformSensor::TickParallel), ETickType::LateTickParallel);
}

References ATickManager::AddTick(), BindTick(), TickEntry, and TickParallel().

◆ CreateDataSavePath()

void ATransformSensor::CreateDataSavePath ( )

overrideprotectedvirtual

Creates data save path for this sensor. Can be overriden if needed.

Reimplemented from ASensor.

Definition at line 279 of file TransformSensor.cpp.

{
    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;
            }
        }
    }
}

References ASensor::FileSavePath, ASensor::GetActorID_Implementation(), UAgrarsensePaths::GetDataFolder(), ASensor::IsAttachedToVehicle(), ASensor::ParentActor, and Vehicle.

Referenced by CreateLogFile().

◆ CreateLogFile()

void ATransformSensor::CreateLogFile ( )

overrideprivatevirtual

Create Text file for this sensor if it has not been created already. Text file will be created to ROOT/Data/Run/Logs directory. Filename will be SensorName_ID_UnixTimestamp.txt Can be overriden by individual sensor if needed.

Reimplemented from ASensor.

Definition at line 237 of file TransformSensor.cpp.

{
    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");
        }
    }
}

References ULogFile::Create(), CreateDataSavePath(), FLogFileSettings::FileCreationOptions, FLogFileSettings::FilePath, ASensor::FileSavePath, FLogFileSettings::FileWriteOptions, ASensor::IsAttachedToVehicle(), FLogFileSettings::KeepFileOpen, ASensor::LogFile, FLogFileSettings::OverrideFilePath, Overwrite, Parameters, Queue, FLogFileSettings::QueueLength, FTransformSensorParameters::SaveTransformDataToDisk, FLogFileSettings::Timestamp, Vehicle, and ASensor::WriteToLogFile().

Referenced by Init().

◆ CreateROSTopic()

void ATransformSensor::CreateROSTopic ( )

overrideprotectedvirtual

Creates ROS Topic for this sensor. Can be overriden by individual sensor if needed.

Reimplemented from ASensor.

Definition at line 69 of file TransformSensor.cpp.

{
    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 NavSatFixTopicName;
 
        int32 LastSlashIndex = BaseTopicName.Find(TEXT("/"), ESearchCase::IgnoreCase, ESearchDir::FromEnd);
        if (LastSlashIndex != INDEX_NONE)
        {
            NavSatFixTopicName = BaseTopicName.Left(LastSlashIndex) + TEXT("/navsatfix");
        }
        else
        {
            NavSatFixTopicName = BaseTopicName + TEXT("/navsatfix");
        }
 
        NavSatFixTopic = NewObject<UTopic>(UTopic::StaticClass());
        if (NavSatFixTopic)
        {
            NavSatFixTopic->Init(ROSInstance->ROSIntegrationCore, NavSatFixTopicName, TEXT("sensor_msgs/NavSatFix"));
            NavSatFixTopic->Advertise();
        }
    }
}

References GeoReferencingSystem, ASensor::GetSensorIdentifier(), NavSatFixTopic, and ASensor::ROSInstance.

Referenced by Init().

◆ DestroyROSTopic()

void ATransformSensor::DestroyROSTopic ( )

overrideprotectedvirtual

Destroy created ROS Topic. Can be overriden by individual sensor if needed.

Reimplemented from ASensor.

Definition at line 100 of file TransformSensor.cpp.

{
    if (NavSatFixTopic)
    {
        NavSatFixTopic->Unadvertise();
        NavSatFixTopic->Unsubscribe();
        NavSatFixTopic->MarkAsDisconnected();
        NavSatFixTopic->ConditionalBeginDestroy();
        NavSatFixTopic = nullptr;
    }
 
    if (ROSTopic)
    {
        ROSTopic->Unadvertise();
        ROSTopic->Unsubscribe();
        ROSTopic->MarkAsDisconnected();
        ROSTopic->ConditionalBeginDestroy();
        ROSTopic = nullptr;
    }
}

References NavSatFixTopic, and ASensor::ROSTopic.

Referenced by EndPlay().

◆ EndPlay()

void ATransformSensor::EndPlay ( const EEndPlayReason::Type EndPlayReason )

overrideprotectedvirtual

Reimplemented from ASensor.

Definition at line 56 of file TransformSensor.cpp.

{
    ATickManager::RemoveTick(TickEntry);
 
    DestroyROSTopic();
 
    Parameters.Clear();
    TransformMessage.Reset();
    NavSatFixMessage.Reset();
 
    Super::EndPlay(EndPlayReason);
}

References FTransformSensorParameters::Clear(), DestroyROSTopic(), NavSatFixMessage, Parameters, ATickManager::RemoveTick(), TickEntry, and TransformMessage.

◆ GetSensorType()

virtual ESensorTypes ATransformSensor::GetSensorType ( ) const

inlineoverridevirtual

Get type of the sensor

Returns: Sensor's type

Reimplemented from ASensor.

Definition at line 40 of file TransformSensor.h.

    {
        return ESensorTypes::Transform;
    }

References Transform.

◆ Init()

void ATransformSensor::Init	(	FTransformSensorParameters	Params,
		bool	SimulateSensor = `true`
	)

Definition at line 22 of file TransformSensor.cpp.

{
    SetActorEnableCollision(false);
 
    SetSimulateSensor(SimulateSensor);
    Parameters = Params;
 
    ROSMessages::geometry_msgs::Transform transformMsg;
    TransformMessage = MakeShared<ROSMessages::geometry_msgs::Transform>(transformMsg);
 
    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();
}

References CreateLogFile(), CreateROSTopic(), NavSatFixMessage, Parameters, ASensor::SetSimulateSensor(), and TransformMessage.

Referenced by USensorFactory::SpawnTransformSensor().

◆ SaveTransformMetaDataToDisk()

void ATransformSensor::SaveTransformMetaDataToDisk	(	const FVector &	Translation,
		const FQuat &	Rotation
	)

private

Definition at line 203 of file TransformSensor.cpp.

{
    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);
}

References ASensor::CreateTimeStampString(), GeoReferencingSystem, ASensor::LogFile, Parameters, FTransformSensorParameters::SaveTransformDataToDisk, UCoordinateConversionUtilities::UnrealToGeographicCoordinates(), and ASensor::WriteToLogFile().

Referenced by TickParallel().

◆ SendGpsDataToROS()

void ATransformSensor::SendGpsDataToROS ( const FVector & ActorPosition )

private

Definition at line 178 of file TransformSensor.cpp.

{
    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 NavSatFix ROS message
    if (NavSatFixTopic && NavSatFixMessage.IsValid())
    {
        NavSatFixMessage->latitude = Latitude;
        NavSatFixMessage->longitude = Longitude;
        NavSatFixMessage->altitude = Altitude;
        NavSatFixMessage->header.time = FROSTime::Now();
 
        NavSatFixTopic->Publish(NavSatFixMessage);
    }
}

References GeoReferencingSystem, NavSatFixMessage, NavSatFixTopic, and UCoordinateConversionUtilities::UnrealToGeographicCoordinates().

Referenced by TickParallel().

◆ SendTransformDataToROS()

void ATransformSensor::SendTransformDataToROS	(	const FVector &	Translation,
		const FQuat &	Rotation
	)

private

Definition at line 166 of file TransformSensor.cpp.

{
    UTopic* Topic = GetROSTopic();
    if (Topic && TransformMessage.IsValid())
    {
        TransformMessage->translation = Translation;
        TransformMessage->rotation = Rotation;
 
        Topic->Publish(TransformMessage);
    }
}

References ASensor::GetROSTopic(), and TransformMessage.

Referenced by TickParallel().

◆ TickParallel()

void ATransformSensor::TickParallel ( float DeltaTime )

private

Definition at line 121 of file TransformSensor.cpp.

{
    if (!CanSimulateSensor())
    {
        return;
    }
 
    FTransform CurrentTransform;
    const bool HasValidTransform =
        (Parameters.UseOwningActorTransform && Parameters.OwningActor && (CurrentTransform = Parameters.OwningActor->GetTransform(), true)) ||
        (Parameters.PrimitiveComponent && Parameters.UsePrimiteRelativeTransform && (CurrentTransform = Parameters.PrimitiveComponent->GetRelativeTransform(), true)) ||
        (Parameters.PrimitiveComponent && !Parameters.UsePrimiteRelativeTransform && (CurrentTransform = Parameters.PrimitiveComponent->GetComponentTransform(), 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);
 
    TimeSinceLastPublish += DeltaTime;
 
    if (PositionChanged || RotationChanged || TimeSinceLastPublish >= 1.0f)
    {
        TimeSinceLastPublish = 0.0f;
        PreviousPosition = CurrentPosition;
        PreviousRotation = CurrentRotation;
 
        if (IsROSConnected())
        {
            SendTransformDataToROS(CurrentPosition, CurrentRotation);
            SendGpsDataToROS(CurrentPosition);
        }
 
        SaveTransformMetaDataToDisk(CurrentPosition, CurrentRotation);
    }
}

References ASensor::CanSimulateSensor(), ASensor::IsROSConnected(), FTransformSensorParameters::OwningActor, Parameters, PreviousPosition, PreviousRotation, FTransformSensorParameters::PrimitiveComponent, SaveTransformMetaDataToDisk(), SendGpsDataToROS(), SendTransformDataToROS(), TimeSinceLastPublish, FTransformSensorParameters::UseOwningActorTransform, and FTransformSensorParameters::UsePrimiteRelativeTransform.

Referenced by BeginPlay().

Member Data Documentation

◆ GeoReferencingSystem

AGeoReferencingSystem* ATransformSensor::GeoReferencingSystem = nullptr

private

Definition at line 88 of file TransformSensor.h.

Referenced by CreateROSTopic(), SaveTransformMetaDataToDisk(), and SendGpsDataToROS().

◆ NavSatFixMessage

TSharedPtr<ROSMessages::sensor_msgs::NavSatFix> ATransformSensor::NavSatFixMessage

private

Definition at line 73 of file TransformSensor.h.

Referenced by EndPlay(), Init(), and SendGpsDataToROS().

◆ NavSatFixTopic

UTopic* ATransformSensor::NavSatFixTopic = nullptr

private

Definition at line 85 of file TransformSensor.h.

Referenced by CreateROSTopic(), DestroyROSTopic(), and SendGpsDataToROS().

◆ Parameters

FTransformSensorParameters ATransformSensor::Parameters

protected

Definition at line 57 of file TransformSensor.h.

Referenced by CreateLogFile(), EndPlay(), Init(), SaveTransformMetaDataToDisk(), and TickParallel().

◆ PreviousPosition

FVector ATransformSensor::PreviousPosition = FVector(0.0f, 0.0f, 0.0f)

private

Definition at line 76 of file TransformSensor.h.

Referenced by TickParallel().

◆ PreviousRotation

FQuat ATransformSensor::PreviousRotation = FQuat(0.0f, 0.0f, 0.0f, 0.0f)

private

Definition at line 79 of file TransformSensor.h.

Referenced by TickParallel().

◆ TickEntry

FTickEntry ATransformSensor::TickEntry

private

Definition at line 82 of file TransformSensor.h.

Referenced by BeginPlay(), and EndPlay().

◆ TimeSinceLastPublish

float ATransformSensor::TimeSinceLastPublish = 0.0f

private

Definition at line 90 of file TransformSensor.h.

Referenced by TickParallel().

◆ TransformMessage

TSharedPtr<ROSMessages::geometry_msgs::Transform> ATransformSensor::TransformMessage

private

Definition at line 71 of file TransformSensor.h.

Referenced by EndPlay(), Init(), and SendTransformDataToROS().

The documentation for this class was generated from the following files:

Unreal/Agrarsense/Source/Agrarsense/Sensor/Transform/TransformSensor.h
Unreal/Agrarsense/Source/Agrarsense/Sensor/Transform/TransformSensor.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ ATransformSensor()

Member Function Documentation

◆ BeginPlay()

◆ CreateDataSavePath()

◆ CreateLogFile()

◆ CreateROSTopic()

◆ DestroyROSTopic()

◆ EndPlay()

◆ GetSensorType()

◆ Init()

◆ SaveTransformMetaDataToDisk()

◆ SendGpsDataToROS()

◆ SendTransformDataToROS()

◆ TickParallel()

Member Data Documentation

◆ GeoReferencingSystem

◆ NavSatFixMessage

◆ NavSatFixTopic

◆ Parameters

◆ PreviousPosition

◆ PreviousRotation

◆ TickEntry

◆ TimeSinceLastPublish

◆ TransformMessage