13#include "ROSIntegration/Classes/ROSIntegrationGameInstance.h"
14#include <geometry_msgs/Twist.h>
16#include "Async/Async.h"
19UBoomMovementSubscriber::UBoomMovementSubscriber()
21 PrimaryComponentTick.bCanEverTick =
false;
24void UBoomMovementSubscriber::BeginPlay()
33 RosHandle->
OnROSStateChanged.AddUniqueDynamic(
this, &UBoomMovementSubscriber::ROSBridgeStateChanged);
37void UBoomMovementSubscriber::OnUnregister()
39 Super::OnUnregister();
46 RosHandle->
OnROSStateChanged.RemoveDynamic(
this, &UBoomMovementSubscriber::ROSBridgeStateChanged);
50void UBoomMovementSubscriber::ROSBridgeStateChanged(
EROSState rosState)
64void UBoomMovementSubscriber::CreateROSTopic()
66 if (BoomMovementTopic)
73 if (ROSInstance && ROSInstance->IsROSConnected())
75 FString VehicleName =
"Vehicle";
78 AActor* OwnerActor = GetOwner();
83 AVehicle* VehiclePtr = Cast<AVehicle>(OwnerActor);
86 VehicleName = IActorInformation::Execute_GetActorID(VehiclePtr);
90 VehicleName = OwnerActor->GetName();
94 BoomMovementTopic = NewObject<UTopic>(UTopic::StaticClass());
95 BoomMovementTopic->Init(ROSInstance->ROSIntegrationCore, FString::Printf(TEXT(
"/agrarsense/in/vehicles/%s/boom_movement"), *VehicleName), TEXT(
"geometry_msgs/Twist"));
96 BoomMovementTopic->Advertise();
99 std::function<void(TSharedPtr<FROSBaseMsg>)> SubscribeCallback = [
this](TSharedPtr<FROSBaseMsg> msg) ->
void
101 auto Concrete = StaticCastSharedPtr<ROSMessages::geometry_msgs::Twist>(msg);
102 if (Concrete.IsValid())
105 Movement.
linear = FVector(Concrete->linear.x, Concrete->linear.y, Concrete->linear.z);
106 Movement.
angular = FVector(Concrete->angular.x, Concrete->angular.y, Concrete->angular.z);
108 AsyncTask(ENamedThreads::GameThread, [
this, Movement]()
110 OnBoomMessageReceived.Broadcast(Movement);
117 BoomMovementTopic->Subscribe(SubscribeCallback);
121void UBoomMovementSubscriber::DestroyROSTopic()
123 if (BoomMovementTopic)
125 BoomMovementTopic->Unadvertise();
126 BoomMovementTopic->Unsubscribe();
127 BoomMovementTopic->MarkAsDisconnected();
128 BoomMovementTopic->ConditionalBeginDestroy();
129 BoomMovementTopic =
nullptr;
static UROSIntegrationGameInstance * GetROSGameInstance(const UObject *WorldContextObject)
static UROSHandler * GetROSHandle(const UObject *WorldContextObject)
FROSDelegate_ROState OnROSStateChanged