Agrarsense
PIDDrone.h
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
6#pragma once
7
8#include "CoreMinimal.h"
9#include "Vehicle.h"
10#include "DroneParameters.h"
11
12#include "Components/SkeletalMeshComponent.h"
13#include "Components/StaticMeshComponent.h"
14
15#include "ROSIntegration/Public/std_msgs/Float32.h"
16
17#include "PIDDrone.generated.h"
18
19class UTopic;
20class AOverlapSensor;
21
22DECLARE_DYNAMIC_MULTICAST_DELEGATE_OneParam(FDroneFinished, FTransform, _LastWaypoint);
23
24UCLASS()
25class AGRARSENSE_API APIDDrone : public AVehicle
26{
27 GENERATED_BODY()
28
29public:
30
31 APIDDrone();
32
37 virtual EVehicleTypes GetVehicleType() const override { return EVehicleTypes::Drone; }
38
39 virtual FTransform GetInteractableWorldTransform_Implementation() const override
40 {
41 if (DroneSkeletalMesh)
42 {
43 return DroneSkeletalMesh->GetComponentTransform();
44 }
45 else
46 {
47 return GetActorTransform();
48 }
49 }
50
54 UFUNCTION(BlueprintCallable, Category = "Drone")
55 void SetDroneAction(EDroneAction mode)
56 {
57 DroneParameters.DroneAction = mode;
58 }
59
60 UPROPERTY(BlueprintAssignable, Category = "Drone")
61 FDroneFinished OnDroneFinished;
62
67 UFUNCTION(BlueprintCallable, Category = "Drone")
69 {
70 return DroneParameters.DroneAction;
71 }
72
73 UFUNCTION(BlueprintCallable, Category = "Drone")
74 FDroneParameters GetDroneParameters() const
75 {
76 return DroneParameters;
77 }
78
79 UFUNCTION(BlueprintImplementableEvent, BlueprintCallable)
80 void ResetDroneEvent();
81
82 UFUNCTION(BlueprintCallable, Category = "Drone")
83 void ChangeDroneParameters(const FDroneParameters& newParameters);
84
85 UFUNCTION(BlueprintCallable, Category = "Drone")
86 void AddWayPoint(FVector Waypoint)
87 {
88 FTransform transform;
89 transform.SetLocation(Waypoint);
90
91 DroneParameters.Points.Add(transform);
92 }
93
94 UFUNCTION(BlueprintCallable, Category = "Drone")
95 float GetDroneDistanceToNextPoint()
96 {
97 // On manual flight, return a fixed distance to not affect autofly max speed
98 if (DroneParameters.DroneAction == EDroneAction::Manual)
99 {
100 return 5000;
101 }
102
103 return distanceToNextPoint;
104 }
105
106 UFUNCTION(BlueprintCallable, Category = "Drone")
107 float GetDroneSpeed() const
108 {
109 if (!DroneSkeletalMesh)
110 {
111 return 0;
112 }
113
114 return FMath::Max(FMath::Abs(DroneSkeletalMesh->GetComponentVelocity().X), FMath::Abs(DroneSkeletalMesh->GetComponentVelocity().Y));
115 }
116
117 UFUNCTION(BlueprintCallable, Category = "Drone")
118 void ClearWaypoints()
119 {
120 DroneParameters.Points.Empty();
121 }
122
126 UFUNCTION(BlueprintCallable, Category = "Drone")
127 void SetFlightpath();
128
129 UFUNCTION(BlueprintCallable, Category = "Drone")
130 void SetManualControl(bool value)
131 {
132 manualControl = value;
133 }
134
139 void MoveDroneToPosition(const FTransform Transform);
140
146 UFUNCTION(BlueprintCallable, Category = "Drone")
147 TArray<FTransform> GenerateRoamingPoints(float radius, int32 roamingPoints);
148
153 UFUNCTION(BlueprintCallable, Category = "Drone")
154 float GetYawRotationDifference(USkeletalMeshComponent* DroneSkeletalMesh, UStaticMeshComponent* DesiredLocation);
155
156 virtual void SetCurrentTransformToMovedFromGarageTransform() override
157 {
158 if (DroneSkeletalMesh)
159 {
160 MovedFromGarageTransform = DroneSkeletalMesh->GetComponentTransform();
161 UE_LOG(LogTemp, Warning, TEXT("Moved from garage transform %s"), *MovedFromGarageTransform.ToString());
162
163 }
164 else
165 {
166 MovedFromGarageTransform = GetActorTransform();
167 UE_LOG(LogTemp, Warning, TEXT("ACTOR Moved from garage transform WHAT IS THIS: %s"), *MovedFromGarageTransform.ToString());
168
169 }
170 }
171
172 void SetShowForwardArrow(bool Show)
173 {
174 ShowForwardArrow = Show;
175 }
176
177 void SetDrawDebugPoints(bool show)
178 {
179 drawDebugPoints = show;
180 }
181
182 UFUNCTION(BlueprintCallable, Category = "Drone")
183 bool GetShowForwardArrow() const
184 {
185 return ShowForwardArrow;
186 }
187
188 UFUNCTION(BlueprintCallable, Category = "Drone")
189 USkeletalMeshComponent* GetDroneSkeletalMesh() const
190 {
191 return DroneSkeletalMesh;
192 }
193
194protected:
195
196 void ROSBridgeStateChanged(EROSState ROSState) override;
197
198 UPROPERTY()
199 UTopic* ROSTopic = nullptr;
200
201 TSharedPtr<ROSMessages::std_msgs::Float32> ROSMessage;
202
203private:
204
205 virtual void BeginPlay() override;
206
207 virtual void Tick(float DeltaTime) override;
208
209 virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override;
210
211 void CreateTopic();
212
213 void DestroyTopic();
214
215 void HandleDestroy();
216
217 void AutoPilot(const float DeltaTime);
218
219 void DrawDebugPoints();
220
221 UFUNCTION(BlueprintCallable)
222 void AssignRoamingPoints(const TArray<FTransform> Points);
223
224 UFUNCTION(BlueprintCallable)
225 void SetDroneRotation(USkeletalMeshComponent* target, FRotator rotator);
226
227 void UpdateGroundHeight();
228
229 UFUNCTION(BlueprintCallable)
230 bool IsRoaming() const
231 {
232 return DroneParameters.DroneAction == EDroneAction::Roaming;
233 }
234
235 UFUNCTION(BlueprintCallable)
236 void SetDesiredLocationMesh(UStaticMeshComponent* meshcomponent)
237 {
238 PositionMesh = meshcomponent;
239 }
240
241 UFUNCTION(BlueprintCallable)
242 FVector GetCurrentWaypointTarget();
243
244 UFUNCTION(BlueprintCallable)
245 FTransform GetCurrentWaypointTarget_Transform();
246
248
249 UPROPERTY()
251
252 UPROPERTY()
253 UWorld* World = nullptr;
254
255 FVector StartingPosition;
256
257 UPROPERTY()
258 USkeletalMeshComponent* DroneSkeletalMesh = nullptr;
259
260 UPROPERTY()
261 UStaticMeshComponent* PositionMesh = nullptr;
262
263 UPROPERTY()
264 AOverlapSensor* InnerOverlapSensor = nullptr;
265
266 /* For testing purposes. Hardcoded coordinates */
267 TArray<FVector> WayPoints = {
268 {3790, 8050, 5000},
269 {11779.536955,6566.541435,5000}
270 };
271
272 bool waypointReached = false;
273
274 bool manualControl = false;
275
276 bool ShowForwardArrow = false;
277
278 bool drawDebugPoints = false;
279
280 int32 passedWaypoints = 0;
281
282 float DroneHeightFromGround = 0.0f;
283
284 float distanceToNextPoint = 0.0f;
285
286 TArray<FTransform> dronePath;
287
288 FTimerHandle DestroyHandle;
289};
EDroneAction
Definition: DroneAction.h:15
EFlightMode
DECLARE_DYNAMIC_MULTICAST_DELEGATE_OneParam(FDroneFinished, FTransform, _LastWaypoint)
EROSState
Definition: ROSState.h:16
static EDroneAction GetDroneAction(FString actionstring)
EVehicleTypes
Definition: VehicleTypes.h:15
TSharedPtr< ROSMessages::std_msgs::String > ROSMessage
virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override
virtual void BeginPlay() override
virtual FTransform GetInteractableWorldTransform_Implementation() const override
Definition: PIDDrone.h:39
void SetDrawDebugPoints(bool show)
Definition: PIDDrone.h:177
FDroneParameters DroneParameters
Definition: PIDDrone.h:247
virtual EVehicleTypes GetVehicleType() const override
Definition: PIDDrone.h:37
TArray< FTransform > dronePath
Definition: PIDDrone.h:286
FTimerHandle DestroyHandle
Definition: PIDDrone.h:288
void SetShowForwardArrow(bool Show)
Definition: PIDDrone.h:172
void ROSBridgeStateChanged(EROSState ROSState)
Definition: Sensor.cpp:169
UTopic * ROSTopic
Definition: Sensor.h:361
TArray< FTransform > Points