13#include "ROSIntegration/Classes/ROSIntegrationGameInstance.h"
14#include <geometry_msgs/Twist.h>
15#include <std_msgs/String.h>
17#include "Async/Async.h"
20UMovementSubscriber::UMovementSubscriber()
22 PrimaryComponentTick.bCanEverTick =
false;
25void UMovementSubscriber::BeginPlay()
34 RosHandle->
OnROSStateChanged.AddUniqueDynamic(
this, &UMovementSubscriber::ROSBridgeStateChanged);
38void UMovementSubscriber::OnUnregister()
40 Super::OnUnregister();
47 RosHandle->
OnROSStateChanged.RemoveDynamic(
this, &UMovementSubscriber::ROSBridgeStateChanged);
51void UMovementSubscriber::ROSBridgeStateChanged(
EROSState rosState)
65void UMovementSubscriber::CreateROSTopic()
74 if (ROSInstance && ROSInstance->IsROSConnected())
76 FString VehicleName =
"Vehicle";
79 AActor* OwnerActor = GetOwner();
84 AVehicle* VehiclePtr = Cast<AVehicle>(OwnerActor);
87 VehicleName = IActorInformation::Execute_GetActorID(VehiclePtr);
91 VehicleName = OwnerActor->GetName();
95 MovementTopic = NewObject<UTopic>(UTopic::StaticClass());
96 MovementTopic->Init(ROSInstance->ROSIntegrationCore, FString::Printf(TEXT(
"/agrarsense/in/vehicles/%s/movement"), *VehicleName), TEXT(
"std_msgs/String"));
97 MovementTopic->Advertise();
99 std::function<void(TSharedPtr<FROSBaseMsg>)> SubscribeCallback = [
this](TSharedPtr<FROSBaseMsg> msg) ->
void
102 auto Concrete = StaticCastSharedPtr<ROSMessages::std_msgs::String>(msg);
103 if (Concrete.IsValid())
106 TArray<FString> TempResult;
107 Concrete->_Data.ParseIntoArray(TempResult, TEXT(
","));
109 if (TempResult.Num() < 3)
113 Movement.
Throttle = TempResult[0].IsNumeric() ? FMath::Clamp(FCString::Atof(*TempResult[0]), -1 , 1) : 0.0f;
114 Movement.
Break = TempResult[1].IsNumeric() ? FMath::Clamp(FCString::Atof(*TempResult[1]), 0, 1) : 0.0f;
115 Movement.
Steering = TempResult[2].IsNumeric() ? FMath::Clamp(FCString::Atof(*TempResult[2]), -90, 90) : 0.0f;
117 AsyncTask(ENamedThreads::GameThread, [
this, Movement]()
119 OnMessageReceived.Broadcast(Movement);
126 MovementTopic->Subscribe(SubscribeCallback);
130void UMovementSubscriber::DestroyROSTopic()
134 MovementTopic->Unadvertise();
135 MovementTopic->Unsubscribe();
136 MovementTopic->MarkAsDisconnected();
137 MovementTopic->ConditionalBeginDestroy();
138 MovementTopic =
nullptr;
static UROSIntegrationGameInstance * GetROSGameInstance(const UObject *WorldContextObject)
static UROSHandler * GetROSHandle(const UObject *WorldContextObject)
FROSDelegate_ROState OnROSStateChanged