Agrarsense
BoomMovementSubscriber.cpp
Go to the documentation of this file.
1// Copyright (c) 2023 FrostBit Software Lab at the Lapland University of Applied Sciences
2//
3// This work is licensed under the terms of the MIT license.
4// For a copy, see <https://opensource.org/licenses/MIT>.
5
7
12
13#include "ROSIntegration/Classes/ROSIntegrationGameInstance.h"
14#include <geometry_msgs/Twist.h>
15
16#include "Async/Async.h"
17#include "AITypes.h"
18
19UBoomMovementSubscriber::UBoomMovementSubscriber()
20{
21 PrimaryComponentTick.bCanEverTick = false;
22}
23
24void UBoomMovementSubscriber::BeginPlay()
25{
26 Super::BeginPlay();
27
28 CreateROSTopic();
29
30 UROSHandler* RosHandle = UAgrarsenseStatics::GetROSHandle(GetWorld());
31 if (RosHandle)
32 {
33 RosHandle->OnROSStateChanged.AddUniqueDynamic(this, &UBoomMovementSubscriber::ROSBridgeStateChanged);
34 }
35}
36
37void UBoomMovementSubscriber::OnUnregister()
38{
39 Super::OnUnregister();
40
41 DestroyROSTopic();
42
43 UROSHandler* RosHandle = UAgrarsenseStatics::GetROSHandle(GetWorld());
44 if (RosHandle)
45 {
46 RosHandle->OnROSStateChanged.RemoveDynamic(this, &UBoomMovementSubscriber::ROSBridgeStateChanged);
47 }
48}
49
50void UBoomMovementSubscriber::ROSBridgeStateChanged(EROSState rosState)
51{
52 switch (rosState)
53 {
55 CreateROSTopic();
56 break;
57
59 DestroyROSTopic();
60 break;
61 }
62}
63
64void UBoomMovementSubscriber::CreateROSTopic()
65{
66 if (BoomMovementTopic)
67 {
68 return;
69 }
70
71 UROSIntegrationGameInstance* ROSInstance = UAgrarsenseStatics::GetROSGameInstance(GetWorld());
72
73 if (ROSInstance && ROSInstance->IsROSConnected())
74 {
75 FString VehicleName = "Vehicle";
76
77 // Get this Actor this component is attached to
78 AActor* OwnerActor = GetOwner();
79 if (OwnerActor)
80 {
81 // Try to cast the owning actor into AVehicle and
82 // get its ID and use the ID as the vehicle name
83 AVehicle* VehiclePtr = Cast<AVehicle>(OwnerActor);
84 if (VehiclePtr)
85 {
86 VehicleName = IActorInformation::Execute_GetActorID(VehiclePtr);
87 }
88 else
89 {
90 VehicleName = OwnerActor->GetName();
91 }
92 }
93
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();
97
98 // Create a std::function callback object
99 std::function<void(TSharedPtr<FROSBaseMsg>)> SubscribeCallback = [this](TSharedPtr<FROSBaseMsg> msg) -> void
100 {
101 auto Concrete = StaticCastSharedPtr<ROSMessages::geometry_msgs::Twist>(msg);
102 if (Concrete.IsValid())
103 {
104 FBoomMessageParser Movement;
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);
107
108 AsyncTask(ENamedThreads::GameThread, [this, Movement]()
109 {
110 OnBoomMessageReceived.Broadcast(Movement);
111 });
112
113 }
114 return;
115 };
116
117 BoomMovementTopic->Subscribe(SubscribeCallback);
118 }
119}
120
121void UBoomMovementSubscriber::DestroyROSTopic()
122{
123 if (BoomMovementTopic)
124 {
125 BoomMovementTopic->Unadvertise();
126 BoomMovementTopic->Unsubscribe();
127 BoomMovementTopic->MarkAsDisconnected();
128 BoomMovementTopic->ConditionalBeginDestroy();
129 BoomMovementTopic = nullptr;
130 }
131}
EROSState
Definition: ROSState.h:16
static UROSIntegrationGameInstance * GetROSGameInstance(const UObject *WorldContextObject)
static UROSHandler * GetROSHandle(const UObject *WorldContextObject)
FROSDelegate_ROState OnROSStateChanged
Definition: ROSHandler.h:81