Agrarsense
MovementSubscriber.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#include <std_msgs/String.h>
16
17#include "Async/Async.h"
18#include "AITypes.h"
19
20UMovementSubscriber::UMovementSubscriber()
21{
22 PrimaryComponentTick.bCanEverTick = false;
23}
24
25void UMovementSubscriber::BeginPlay()
26{
27 Super::BeginPlay();
28
29 CreateROSTopic();
30
31 UROSHandler* RosHandle = UAgrarsenseStatics::GetROSHandle(GetWorld());
32 if (RosHandle)
33 {
34 RosHandle->OnROSStateChanged.AddUniqueDynamic(this, &UMovementSubscriber::ROSBridgeStateChanged);
35 }
36}
37
38void UMovementSubscriber::OnUnregister()
39{
40 Super::OnUnregister();
41
42 DestroyROSTopic();
43
44 UROSHandler* RosHandle = UAgrarsenseStatics::GetROSHandle(GetWorld());
45 if (RosHandle)
46 {
47 RosHandle->OnROSStateChanged.RemoveDynamic(this, &UMovementSubscriber::ROSBridgeStateChanged);
48 }
49}
50
51void UMovementSubscriber::ROSBridgeStateChanged(EROSState rosState)
52{
53 switch (rosState)
54 {
56 CreateROSTopic();
57 break;
58
60 DestroyROSTopic();
61 break;
62 }
63}
64
65void UMovementSubscriber::CreateROSTopic()
66{
67 if (MovementTopic)
68 {
69 return;
70 }
71
72 UROSIntegrationGameInstance* ROSInstance = UAgrarsenseStatics::GetROSGameInstance(GetWorld());
73
74 if (ROSInstance && ROSInstance->IsROSConnected())
75 {
76 FString VehicleName = "Vehicle";
77
78 // Get this Actor this component is attached to
79 AActor* OwnerActor = GetOwner();
80 if (OwnerActor)
81 {
82 // Try to cast the owning actor into AVehicle and
83 // get its ID and use the ID as the vehicle name
84 AVehicle* VehiclePtr = Cast<AVehicle>(OwnerActor);
85 if (VehiclePtr)
86 {
87 VehicleName = IActorInformation::Execute_GetActorID(VehiclePtr);
88 }
89 else
90 {
91 VehicleName = OwnerActor->GetName();
92 }
93 }
94
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();
98
99 std::function<void(TSharedPtr<FROSBaseMsg>)> SubscribeCallback = [this](TSharedPtr<FROSBaseMsg> msg) -> void
100 {
101 //auto Concrete = StaticCastSharedPtr<ROSMessages::geometry_msgs::Twist>(msg);
102 auto Concrete = StaticCastSharedPtr<ROSMessages::std_msgs::String>(msg);
103 if (Concrete.IsValid())
104 {
105 // TempResult[0] = Throttle, TempResult[1] = Break, TempResult[2] = Steering
106 TArray<FString> TempResult;
107 Concrete->_Data.ParseIntoArray(TempResult, TEXT(","));
108
109 if (TempResult.Num() < 3)
110 return;
111
112 FMessageParser Movement;
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;
116
117 AsyncTask(ENamedThreads::GameThread, [this, Movement]()
118 {
119 OnMessageReceived.Broadcast(Movement);
120 });
121
122 }
123 return;
124 };
125
126 MovementTopic->Subscribe(SubscribeCallback);
127 }
128}
129
130void UMovementSubscriber::DestroyROSTopic()
131{
132 if (MovementTopic)
133 {
134 MovementTopic->Unadvertise();
135 MovementTopic->Unsubscribe();
136 MovementTopic->MarkAsDisconnected();
137 MovementTopic->ConditionalBeginDestroy();
138 MovementTopic = nullptr;
139 }
140}
EROSState
Definition: ROSState.h:16
static UROSIntegrationGameInstance * GetROSGameInstance(const UObject *WorldContextObject)
static UROSHandler * GetROSHandle(const UObject *WorldContextObject)
FROSDelegate_ROState OnROSStateChanged
Definition: ROSHandler.h:81