MovementSubscriber.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 "MovementSubscriber.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 <std_msgs/String.h>
 
#include "Async/Async.h"
#include "AITypes.h"
 
UMovementSubscriber::UMovementSubscriber()
{
    PrimaryComponentTick.bCanEverTick = false;
}
 
void UMovementSubscriber::BeginPlay()
{
    Super::BeginPlay();
 
    CreateROSTopic();
 
    UROSHandler* RosHandle = UAgrarsenseStatics::GetROSHandle(GetWorld());
    if (RosHandle)
    {
        RosHandle->OnROSStateChanged.AddUniqueDynamic(this, &UMovementSubscriber::ROSBridgeStateChanged);
    }
}
 
void UMovementSubscriber::OnUnregister()
{
    Super::OnUnregister();
 
    DestroyROSTopic();
 
    UROSHandler* RosHandle = UAgrarsenseStatics::GetROSHandle(GetWorld());
    if (RosHandle)
    {
        RosHandle->OnROSStateChanged.RemoveDynamic(this, &UMovementSubscriber::ROSBridgeStateChanged);
    }
}
 
void UMovementSubscriber::ROSBridgeStateChanged(EROSState rosState)
{
    switch (rosState)
    {
    case EROSState::Connected:
        CreateROSTopic();
        break;
 
    case EROSState::Disconnected:
        DestroyROSTopic();
        break;
    }
}
 
void UMovementSubscriber::CreateROSTopic()
{
    if (MovementTopic) 
    {
        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();
            }
        }
 
        MovementTopic = NewObject<UTopic>(UTopic::StaticClass());
        MovementTopic->Init(ROSInstance->ROSIntegrationCore, FString::Printf(TEXT("/agrarsense/in/vehicles/%s/movement"), *VehicleName), TEXT("std_msgs/String"));
        MovementTopic->Advertise();
        
        std::function<void(TSharedPtr<FROSBaseMsg>)> SubscribeCallback = [this](TSharedPtr<FROSBaseMsg> msg) -> void
        {
            //auto Concrete = StaticCastSharedPtr<ROSMessages::geometry_msgs::Twist>(msg);
            auto Concrete = StaticCastSharedPtr<ROSMessages::std_msgs::String>(msg);
            if (Concrete.IsValid())
            {
                // TempResult[0] = Throttle, TempResult[1] = Break, TempResult[2] = Steering
                TArray<FString> TempResult;
                Concrete->_Data.ParseIntoArray(TempResult, TEXT(","));
 
                if (TempResult.Num() < 3)
                    return;
 
                FMessageParser Movement;
                Movement.Throttle = TempResult[0].IsNumeric() ? FMath::Clamp(FCString::Atof(*TempResult[0]), -1 , 1) : 0.0f;
                Movement.Break = TempResult[1].IsNumeric() ? FMath::Clamp(FCString::Atof(*TempResult[1]), 0, 1) : 0.0f;
                Movement.Steering = TempResult[2].IsNumeric() ? FMath::Clamp(FCString::Atof(*TempResult[2]), -90, 90) : 0.0f;
 
                AsyncTask(ENamedThreads::GameThread, [this, Movement]()
                {
                    OnMessageReceived.Broadcast(Movement);
                });
                
            }
            return;
        };
 
        MovementTopic->Subscribe(SubscribeCallback);
    }
}
 
void UMovementSubscriber::DestroyROSTopic()
{
    if (MovementTopic)
    {
        MovementTopic->Unadvertise();
        MovementTopic->Unsubscribe();
        MovementTopic->MarkAsDisconnected();
        MovementTopic->ConditionalBeginDestroy();
        MovementTopic = nullptr;
    }
}