BoomMovementSubscriber.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 "BoomMovementSubscriber.h"
 
#include "Agrarsense/Game/AgrarsenseStatics.h"
#include "Agrarsense/Vehicle/Vehicle.h"
#include "Agrarsense/ROS/ROSHandler.h"
 
#include "ROSIntegration/Classes/ROSIntegrationGameInstance.h"
#include <geometry_msgs/Twist.h>
 
#include "Async/Async.h"
#include "AITypes.h"
 
UBoomMovementSubscriber::UBoomMovementSubscriber()
{
    PrimaryComponentTick.bCanEverTick = false;
}
 
void UBoomMovementSubscriber::BeginPlay()
{
    Super::BeginPlay();
 
    CreateROSTopic();
 
    UROSHandler* RosHandle = UAgrarsenseStatics::GetROSHandle(GetWorld());
    if (RosHandle)
    {
        RosHandle->OnROSStateChanged.AddUniqueDynamic(this, &UBoomMovementSubscriber::ROSBridgeStateChanged);
    }
}
 
void UBoomMovementSubscriber::OnUnregister()
{
    Super::OnUnregister();
 
    DestroyROSTopic();
 
    UROSHandler* RosHandle = UAgrarsenseStatics::GetROSHandle(GetWorld());
    if (RosHandle)
    {
        RosHandle->OnROSStateChanged.RemoveDynamic(this, &UBoomMovementSubscriber::ROSBridgeStateChanged);
    }
}
 
void UBoomMovementSubscriber::ROSBridgeStateChanged(EROSState rosState)
{
    switch (rosState)
    {
    case EROSState::Connected:
        CreateROSTopic();
        break;
 
    case EROSState::Disconnected:
        DestroyROSTopic();
        break;
    }
}
 
void UBoomMovementSubscriber::CreateROSTopic()
{
    if (BoomMovementTopic)
    {
        return;
    }
 
    UROSIntegrationGameInstance* ROSInstance = UAgrarsenseStatics::GetROSGameInstance(GetWorld());
 
    if (ROSInstance && ROSInstance->IsROSConnected())
    {
        FString VehicleName = "Vehicle";
 
        // Get this Actor this component is attached to
        AActor* OwnerActor = GetOwner();
        if (OwnerActor)
        {
            // Try to cast the owning actor into AVehicle and 
            // get its ID and use the ID as the vehicle name
            AVehicle* VehiclePtr = Cast<AVehicle>(OwnerActor);
            if (VehiclePtr)
            {
                VehicleName = VehiclePtr->GetActorID_Implementation();
            }
            else 
            {
                VehicleName = OwnerActor->GetName();
            }
        }
 
        BoomMovementTopic = NewObject<UTopic>(UTopic::StaticClass());
        BoomMovementTopic->Init(ROSInstance->ROSIntegrationCore, FString::Printf(TEXT("/agrarsense/in/vehicles/%s/boom_movement"), *VehicleName), TEXT("geometry_msgs/Twist"));
        BoomMovementTopic->Advertise();
 
        // Create a std::function callback object
        std::function<void(TSharedPtr<FROSBaseMsg>)> SubscribeCallback = [this](TSharedPtr<FROSBaseMsg> msg) -> void
        {
            auto Concrete = StaticCastSharedPtr<ROSMessages::geometry_msgs::Twist>(msg);
            if (Concrete.IsValid())
            {
                FBoomMessageParser Movement;
                Movement.linear = FVector(Concrete->linear.x, Concrete->linear.y, Concrete->linear.z);
                Movement.angular = FVector(Concrete->angular.x, Concrete->angular.y, Concrete->angular.z);
 
                AsyncTask(ENamedThreads::GameThread, [this, Movement]()
                    {
                        OnBoomMessageReceived.Broadcast(Movement);
                    });
 
            }
            return;
        };
 
        BoomMovementTopic->Subscribe(SubscribeCallback);
    }
}
 
void UBoomMovementSubscriber::DestroyROSTopic()
{
    if (BoomMovementTopic)
    {
        BoomMovementTopic->Unadvertise();
        BoomMovementTopic->Unsubscribe();
        BoomMovementTopic->MarkAsDisconnected();
        BoomMovementTopic->ConditionalBeginDestroy();
        BoomMovementTopic = nullptr;
    }
}