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/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(AActor* TransformActor, bool SimulateSensor)
{
    SetSimulateSensor(SimulateSensor);
    Target = TransformActor;
 
    ROSMessages::geometry_msgs::Transform transformMsg;
    TransformMessage = MakeShared<ROSMessages::geometry_msgs::Transform>(transformMsg);
 
    GnssMessageString = MakeShared<ROSMessages::std_msgs::String>();
 
    CreateROSTopic();
}
 
void ATransformSensor::BeginPlay()
{
    Super::BeginPlay();
 
    SetActorEnableCollision(false);
    PreviousActorPosition = GetActorLocation();
 
    TickEntry = ATickManager::AddTick(this, BindTick(this, &ATransformSensor::TickParallel), ETickType::LateTickParallel);
}
 
void ATransformSensor::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    ATickManager::RemoveTick(TickEntry);
 
    DestroyROSTopic();
    GnssMessageString.Reset();
    TransformMessage.Reset();
    Target.Reset();
 
    Super::EndPlay(EndPlayReason);
}
 
void ATransformSensor::CreateROSTopic()
{
    Super::CreateROSTopic();
 
    GeoReferencingSystem = AGeoReferencingSystem::GetGeoReferencingSystem(GetWorld());
 
    // If map has been georeferenced, create gnss topic.
    if (GeoReferencingSystem)
    {
        FString TopicName = FString::Printf(TEXT("/agrarsense/out/sensors/%s"), *GetSensorIdentifier());
        int32 LastSlashIndex = TopicName.Find(TEXT("/"), ESearchCase::IgnoreCase, ESearchDir::FromEnd);
        if (LastSlashIndex != INDEX_NONE)
        {
            TopicName = TopicName.Left(LastSlashIndex) + TEXT("/gnss");
        }
        else
        {
            TopicName += TEXT("/gnss");
        }
 
        GnssTopic = NewObject<UTopic>(UTopic::StaticClass());
        if (GnssTopic)
        {
            GnssTopic->Init(ROSInstance->ROSIntegrationCore, TopicName, TEXT("std_msgs/String"));
            GnssTopic->Advertise();
        }
    }
}
 
void ATransformSensor::DestroyROSTopic()
{
    if (GnssTopic)
    {
        GnssTopic->Unadvertise();
        GnssTopic->Unsubscribe();
        GnssTopic->MarkAsDisconnected();
        GnssTopic->ConditionalBeginDestroy();
        GnssTopic = nullptr;
    }
 
    if (ROSTopic)
    {
        ROSTopic->Unadvertise();
        ROSTopic->Unsubscribe();
        ROSTopic->MarkAsDisconnected();
        ROSTopic->ConditionalBeginDestroy();
        ROSTopic = nullptr;
    }
}
 
void ATransformSensor::TickParallel(float DeltaTime)
{
    AActor* TargetActor = Target.Get();
    if (!CanSimulateSensor() || !TargetActor)
    {
        return;
    }
 
    FTransform CurrentTransform = TargetActor->GetTransform();
    FVector CurrentPos = CurrentTransform.GetLocation();
    FQuat CurrentRot = CurrentTransform.GetRotation();
 
    if (CurrentPos != PreviousActorPosition)
    {
        // Send transform data to ROS topic,
        // this is done frequently but this should be quite fast and 
        // should give precise actor's current location and rotation in the World.
        PreviousActorPosition = CurrentPos;
 
        if (IsROSConnected()) 
        {
            SendTransformDataToROS(CurrentPos, CurrentRot);
            SendGnssDataToROS(CurrentPos);
        }
    }
}
 
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::SendGnssDataToROS(const FVector& ActorPosition)
{
    if (GeoReferencingSystem && GnssTopic && GnssMessageString.IsValid())
    {
        // Convert Actor position to FGeographicCoordinates
        FGeographicCoordinates GeographicCoordinates = UCoordinateConversionUtilities::UnrealToGeographicCoordinates(GeoReferencingSystem, ActorPosition);
 
        double Latitude = GeographicCoordinates.Latitude;
        double Longitude = GeographicCoordinates.Longitude;
        double Altitude = GeographicCoordinates.Altitude;
 
        // Create gnss string and send the message to the created ROS topic
        FString GnssDataString = FString::Printf(TEXT("%f, %f, %f"), Latitude, Longitude, Altitude);
        GnssMessageString->_Data = GnssDataString;
        GnssTopic->Publish(GnssMessageString);
    }
}