InfoTopic.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 "InfoTopic.h"
 
#include "Agrarsense/Game/AgrarsenseStatics.h"
#include "Agrarsense/ROS/ROSHandler.h"
 
#include "ROSIntegration/Classes/ROSIntegrationGameInstance.h"
#include "ROSIntegration/Classes/RI/Topic.h"
 
#include "Engine/GameViewportClient.h"
#include "Engine/Engine.h"
#include "Engine/World.h"
 
TSharedPtr<ROSMessages::std_msgs::String> UInfoTopic::ROSMessage;
UInfoTopic* UInfoTopic::Instance = nullptr;
TWeakObjectPtr<UTopic> UInfoTopic::InfoTopic;
bool UInfoTopic::AllowInfoTopic = true;
bool UInfoTopic::ROSConnected = false;
 
void UInfoTopic::Init()
{
    if (Instance == nullptr)
    {
        Instance = NewObject<UInfoTopic>();
        Instance->Setup();
    }
}
 
void UInfoTopic::Destroy()
{
    if (Instance)
    {
        Instance->DestroyROSTopic();
        Instance = nullptr;
    }
}
 
void UInfoTopic::Setup()
{
    Instance = this;
 
    if (GEngine && GEngine->GameViewport)
    {
        UWorld* World = GEngine->GameViewport->GetWorld();
        UROSHandler* RosHandle = UAgrarsenseStatics::GetROSHandle(World);
        if (RosHandle)
        {
            RosHandle->OnROSStateChanged.AddUniqueDynamic(this, &UInfoTopic::ROSBridgeStateChanged);
        }
    }
 
    SetupROSInfoTopic();
}
 
void UInfoTopic::ROSBridgeStateChanged(EROSState state)
{
    switch (state)
    {
    case EROSState::Connected:
        ROSConnected = true;
        SetupROSInfoTopic();
        break;
 
    case EROSState::Disconnected:
        ROSConnected = false;
        DestroyROSTopic();
        break;
    }
}
 
void UInfoTopic::DestroyROSTopic()
{
    if (InfoTopic.IsValid())
    {
        InfoTopic->Unadvertise();
        InfoTopic->Unsubscribe();
        InfoTopic->MarkAsDisconnected();
        InfoTopic->ConditionalBeginDestroy();
        InfoTopic.Reset();
 
#if WITH_EDITOR
        UE_LOG(LogTemp, Warning, TEXT("InfoTopic.cpp: Destroyed ROS Info Topic"));
#endif
    }
}
 
void UInfoTopic::SetupROSInfoTopic()
{
    if (InfoTopic.IsValid() && ROSMessage.IsValid())
    {
        return;
    }
 
    if (GEngine && GEngine->GameViewport)
    {
        UWorld* World = GEngine->GameViewport->GetWorld();
        UROSIntegrationGameInstance* RosInstance = UAgrarsenseStatics::GetROSGameInstance(World);
 
        if (RosInstance)
        {
            ROSMessage.Reset();
            ROSMessage = MakeShared<ROSMessages::std_msgs::String>();
 
            ROSConnected = RosInstance->IsROSConnected();
 
            if (!InfoTopic.IsValid() && ROSConnected)
            {
                InfoTopic = NewObject<UTopic>(UTopic::StaticClass());
                InfoTopic->Init(RosInstance->ROSIntegrationCore, TEXT("/agrarsense/out/info"), TEXT("std_msgs/String"));
                InfoTopic->Advertise();
 
#if WITH_EDITOR
                UE_LOG(LogTemp, Warning, TEXT("InfoTopic.cpp: Created ROS Info Topic"));
#endif
            }
        }
    }
}
 
void UInfoTopic::SetAllowSendInfo(bool Allow)
{
    AllowInfoTopic = Allow;
}
 
void UInfoTopic::SendMessage(const FString& Message)
{
    // UInfoTopic::Init must be called before SendMessage is called.
    // At the moment Init called from AgrarsenseGameModeBase.cpp BeginPlay
 
    if (!InfoTopic.IsValid() || !ROSMessage.IsValid())
    {
        if (Instance)
        {
            Instance->SetupROSInfoTopic();
        }
    }
 
    if (AllowInfoTopic && ROSConnected && Instance
        && InfoTopic.IsValid() && ROSMessage.IsValid() && !Message.IsEmpty())
    {
        ROSMessage->_Data = Message;
        InfoTopic->Publish(ROSMessage);
    }
}