Agrarsense
PIDDrone.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
6#include "PIDDrone.h"
7
12
18
19#include "ROSIntegration/Classes/RI/Topic.h"
20
21#include "Camera/CameraComponent.h"
22#include "Field/FieldSystemNodes.h"
23#include "NiagaraComponent.h"
24#include "Algo/Reverse.h"
25
26#if WITH_EDITOR
27#include "DrawDebugHelpers.h"
28#endif
29
31{
32 PrimaryActorTick.bCanEverTick = true;
33 InteractableName = NSLOCTEXT("Agrarsense", "DroneInteractableName", "Drone");
34}
35
37{
38 DroneParameters = newParameters;
39
41 {
42 FString OverlapSensorID = ActorID + "/inner_overlap";
43
44 FSensorSpawnParameters SpawnParams;
45 SpawnParams.Transform = GetActorTransform();
46 SpawnParams.SensorIdentifier = OverlapSensorID;
47 SpawnParams.SensorName = "overlap";
48 SpawnParams.SimulateSensor = true;
49 SpawnParams.Parent = this;
50
51 FOverlapSensorParameters SensorParams;
52 SensorParams.OwningActor = this;
53 SensorParams.AllChannels = true;
55 SensorParams.RelativePosition = FVector(0.0f, 0.0f, 0.0f);
56
57 InnerOverlapSensor = USensorFactory::SpawnOverlapSensor(SpawnParams, SensorParams);
58
60 {
61 InnerOverlapSensor->AttachToComponent(DroneSkeletalMesh, FAttachmentTransformRules::KeepRelativeTransform);
62 }
63 }
65 {
66 InnerOverlapSensor->Destroy();
67 InnerOverlapSensor = nullptr;
68 }
69
71 {
72 DroneSkeletalMesh->SetCollisionProfileName(TEXT("DroneNoCollision"));
73 }
74
75 if (OverlapSensor)
76 {
78
79 const float BoundsSizeMeters = DroneParameters.OverlapRadiusMeters;
80 OverlapSensor->SetOverlapBounds(FVector(BoundsSizeMeters, BoundsSizeMeters, BoundsSizeMeters));
81
82 }
83
85 {
87
88 const float BoundsSizeMeters = DroneParameters.InnerOverlapRadiusMeters;
89 InnerOverlapSensor->SetOverlapBounds(FVector(BoundsSizeMeters, BoundsSizeMeters, BoundsSizeMeters));
90 }
91
92 if (!CollisionSensor)
93 {
94 // Create Collision sensor
95 FString VehicleCollisionSensorID = ActorID + "/collision";
96 FSensorSpawnParameters CollisionSensorSpawnParams;
97 CollisionSensorSpawnParams.Transform = GetActorTransform();
98 CollisionSensorSpawnParams.SensorIdentifier = VehicleCollisionSensorID;
99 CollisionSensorSpawnParams.SensorName = "collision";
100 CollisionSensorSpawnParams.SimulateSensor = true;
101 CollisionSensorSpawnParams.Parent = this;
102
103 FCollisionSensorParameters FCollisionSensorSpawnParams;
104 FCollisionSensorSpawnParams.OwningActor = this;
105 FCollisionSensorSpawnParams.UseActorCollision = false;
106 FCollisionSensorSpawnParams.PrimitiveComponent = DroneSkeletalMesh;
107
108 CollisionSensor = USensorFactory::SpawnCollisionSensor(CollisionSensorSpawnParams, FCollisionSensorSpawnParams);
109 }
110}
111
113{
114 Super::BeginPlay();
115
116 World = GetWorld();
117
118 ROSMessage.Reset();
119 ROSMessage = MakeShared<ROSMessages::std_msgs::Float32>();
120
121 CreateTopic();
122
123 // These should be setup in BP_Drone_PID blueprint.
124 DroneSkeletalMesh = Cast<USkeletalMeshComponent>(GetComponentByClass(USkeletalMeshComponent::StaticClass()));
125 PositionMesh = Cast<UStaticMeshComponent>(GetComponentByClass(UStaticMeshComponent::StaticClass()));
126
127 StartingPosition = GetActorLocation();
128 FTransform ActorTransform = GetActorTransform();
129
131 {
132 // Create Transform sensor for forwarder and harvester
133
134 FString VehicleTransformSensorID = ActorID + "/transform";
135
136 FSensorSpawnParameters TransformSensorSpawnParams;
137 TransformSensorSpawnParams.Transform = GetActorTransform();
138 TransformSensorSpawnParams.SensorIdentifier = VehicleTransformSensorID;
139 TransformSensorSpawnParams.SensorName = "transform";
140 TransformSensorSpawnParams.SimulateSensor = true;
141 TransformSensorSpawnParams.Parent = this;
142
143 FTransformSensorParameters TransformSensorParams;
144 TransformSensorParams.SaveTransformDataToDisk = true;
145 TransformSensorParams.OwningActor = this;
146 TransformSensorParams.UseOwningActorTransform = false;
147 TransformSensorParams.PrimitiveComponent = Cast<UPrimitiveComponent>(DroneSkeletalMesh);
148
149 TransformSensor = USensorFactory::SpawnTransformSensor(TransformSensorSpawnParams, TransformSensorParams);
150 }
151
152 if (!OverlapSensor)
153 {
154 // Create OverlapSensor
155
156 FString OverlapSensorID = ActorID + "/overlap";
157
158 FSensorSpawnParameters OverlapSensorSpawnParams;
159 OverlapSensorSpawnParams.Transform = GetActorTransform();
160 OverlapSensorSpawnParams.SensorIdentifier = OverlapSensorID;
161 OverlapSensorSpawnParams.SensorName = "overlap";
162 OverlapSensorSpawnParams.SimulateSensor = true;
163 OverlapSensorSpawnParams.Parent = this;
164
165 FOverlapSensorParameters OverlapSensorParams;
166 OverlapSensorParams.OwningActor = this;
167 OverlapSensorParams.AllChannels = true;
168 OverlapSensorParams.Size = FVector(25.0f, 25.0f, 25.0f);
169 OverlapSensorParams.RelativePosition = FVector(0.0f, 0.0f, 0.0f);
170
171 OverlapSensor = USensorFactory::SpawnOverlapSensor(OverlapSensorSpawnParams, OverlapSensorParams);
172
173 if (OverlapSensor)
174 {
175 OverlapSensor->AttachToComponent(DroneSkeletalMesh, FAttachmentTransformRules::KeepRelativeTransform);
176 }
177 }
178
180 {
181 // Attach rain/snowfall niagara component to drone skeletal mesh.
182 NiagaraComponent->AttachToComponent(DroneSkeletalMesh, FAttachmentTransformRules::KeepRelativeTransform);
183 }
184
185 // Apply default parameters
187}
188
189void APIDDrone::EndPlay(const EEndPlayReason::Type EndPlayReason)
190{
191 Super::EndPlay(EndPlayReason);
192
193 if (TransformSensor)
194 {
195 TransformSensor->Destroy();
196 TransformSensor = nullptr;
197 }
198
200 {
201 InnerOverlapSensor->Destroy();
202 InnerOverlapSensor = nullptr;
203 }
204
205 ROSMessage.Reset();
206 DestroyTopic();
207}
208
209void APIDDrone::Tick(float DeltaTime)
210{
211 Super::Tick(DeltaTime);
212
213 if (!IsVehicleInGarage())
214 {
215 AutoPilot(DeltaTime);
217
218#if WITH_EDITOR
219 if (drawDebugPoints)
220 {
222 }
223#endif
224
225 }
226}
227
228void APIDDrone::AutoPilot(const float DeltaTime)
229{
231 {
232 return;
233 }
234
236}
237
239{
240 TArray<FTransform> dronePoints = DroneParameters.Points;
241
242 dronePath.Add(DroneSkeletalMesh->GetComponentTransform());
243
244 for (int32 i = 0; i < dronePoints.Num(); i++)
245 {
246 if (World)
247 {
248 //UE_LOG(LogTemp, Warning, TEXT("Drone point %d: %s"), i, *dronePoints[i].GetLocation().ToString());
249 DrawDebugSphere(World, dronePoints[i].GetLocation(), 5.0f, 25, FColor::Red, false);
250 if (i < dronePoints.Num() - 1)
251 {
252 FVector nextPointLocation = dronePoints[i + 1].GetLocation();
253 DrawDebugLine(World, dronePoints[i].GetLocation(), nextPointLocation, FColor::Blue, false, 0.0f, 0, 5.0f);
254 }
255 }
256 }
257
258 for (int32 i = 0; i < dronePath.Num(); i++)
259 {
260 if (i < dronePath.Num() - 1)
261 {
262 FVector nextPointLocation = dronePath[i + 1].GetLocation();
263 DrawDebugLine(World, dronePath[i].GetLocation(), nextPointLocation, FColor::Green, false, 0.0f, 0, 5.0f);
264 }
265 }
266}
267
269{
270 // Should trigger EndPlay?
271
273 if (SimulationLevelManager)
274 {
275 AVehicle* controlledVehicle = SimulationLevelManager->GetManuallyControlledVehicle();
276
277 if (controlledVehicle == this)
278 {
279 SimulationLevelManager->CeaseManualControlOfVehicle();
280 }
281 }
282
283 passedWaypoints = 0;
284 Destroy();
285}
286
287
289{
290 // TODO: Check if in garage
291 // include drone rotation to point
292
293 // Don't try to fly if no waypoints
294 if (DroneParameters.Points.Num() == 0 && IsRoaming())
295 {
297 }
298
299 FVector waypoint = GetCurrentWaypointTarget();
300 FVector currentlocation = DroneSkeletalMesh->GetRelativeTransform().GetLocation();
301
302 if (PositionMesh)
303 {
304 // Doesn't work
305 PositionMesh->SetWorldLocation(waypoint);
306 }
307#if WITH_EDITOR
308 else
309 {
310 UE_LOG(LogTemp, Warning, TEXT("Desired location mesh not found"));
311 //DrawDebugLine(World, currentlocation, waypoint, FColor::Red, false, 0.0f, 0.0f, 5.0f);
312 }
313#endif
314 distanceToNextPoint = FVector2f::Distance(FVector2f(waypoint.X, waypoint.Y), FVector2f(currentlocation.X, currentlocation.Y));
315 if (distanceToNextPoint < 100)
316 {
317 waypointReached = true;
318 if (passedWaypoints != DroneParameters.Points.Num() - 1)
319 {
320#if WITH_EDITOR
321 UE_LOG(LogTemp, Warning, TEXT("Changing waypoint from %i to %i"), passedWaypoints, passedWaypoints + 1);
322#endif
323
325 }
326 else
327 {
329 {
331 passedWaypoints = 0;
332 }
333 else
334 {
336 {
339 OnDroneFinished.Broadcast(DroneParameters.Points.Last());
340 break;
343 passedWaypoints = 0;
344 break;
346 passedWaypoints = 0;
347 break;
349 Algo::Reverse(DroneParameters.Points);
350 passedWaypoints = 0;
351 break;
353 OnDroneFinished.Broadcast(DroneParameters.Points.Last());
354 if (!GetWorld()->GetTimerManager().IsTimerActive(DestroyHandle))
355 {
357 GetWorld()->GetTimerManager().SetTimer(DestroyHandle, this, &APIDDrone::HandleDestroy, 5.0f, false);
358 }
359 break;
360 default:
361 break;
362 }
363 }
364 }
365 }
366}
367
368void APIDDrone::MoveDroneToPosition(const FTransform Transform)
369{
370 if (DroneParameters.Points.Num() > 0)
371 {
372 DroneParameters.Points.Empty();
374 }
375 else
376 {
378 }
379}
380
382{
383 return GetCurrentWaypointTarget_Transform().GetLocation();
384}
385
387{
389 {
390 return DroneSkeletalMesh->GetRelativeTransform();
391 }
392
394}
395
396void APIDDrone::AssignRoamingPoints(const TArray<FTransform> Points)
397{
399
400 for (const FTransform& point : Points)
401 {
402 WayPoints.Add(point.GetLocation());
403 }
404}
405
406void APIDDrone::SetDroneRotation(USkeletalMeshComponent* target, FRotator rotator)
407{
408 if (!target)
409 {
410 return;
411 }
412
413 FRotator currentRotation = target->GetRelativeRotation();
414 FRotator rotationDifference = rotator - currentRotation;
415
416 target->AddRelativeRotation(rotationDifference, false, nullptr, ETeleportType::TeleportPhysics);
417
418 // Prevent locking up when using interp
419 if (FMath::Abs(rotator.Pitch) < 0.001f)
420 {
421 rotator.Pitch = 0.0f;
422 }
423 if (FMath::Abs(rotator.Yaw) < 0.001f)
424 {
425 rotator.Yaw = 0.0f;
426 }
427 if (FMath::Abs(rotator.Roll) < 0.001f)
428 {
429 rotator.Roll = 0.0f;
430 }
431
432 // Corrected rotation
433 target->SetRelativeRotation(rotator, false, nullptr, ETeleportType::TeleportPhysics);
434}
435
437{
438 if (!DroneSkeletalMesh || !ROSTopic || !ROSMessage.IsValid())
439 {
440 return;
441 }
442
443 FVector Start = DroneSkeletalMesh->GetComponentLocation();
444 FVector End = Start - FVector(0, 0, 10000.0f);
445
446 FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("UpdateGroundHeight")), true, this);
447 TraceParams.bReturnPhysicalMaterial = false;
448 TraceParams.bTraceComplex = false;
449
450 FHitResult HitResult;
451
452#ifdef ParallelLineTraceSingleByChannel_EXISTS
453 // Defined and implemented in our AGRARSENSE Unreal Engine fork
454 // This LineTrace method doesn't block the physics scene which improves linetrace performance.
455 World->ParallelLineTraceSingleByChannel(
456 HitResult,
457 Start,
458 End,
459 ECC_Visibility,
460 TraceParams,
461 FCollisionResponseParams::DefaultResponseParam);
462#else
463 // If not using our fork of the engine, use default LineTrace method.
464 World->LineTraceSingleByChannel(
465 HitResult,
466 Start,
467 End,
468 ECC_Visibility,
469 TraceParams,
470 FCollisionResponseParams::DefaultResponseParam);
471#endif
472
473 if (HitResult.IsValidBlockingHit())
474 {
475 float Height = (Start.Z - HitResult.ImpactPoint.Z) / 100.0f;
476 float HeightRounded = FMath::RoundToFloat(Height * 1000.0f) / 1000.0f;
477
478 if (!FMath::IsNearlyEqual(DroneHeightFromGround, HeightRounded, 0.005f))
479 {
480 DroneHeightFromGround = HeightRounded;
481
483 ROSTopic->Publish(ROSMessage);
484
485 //FString HitActorName = HitResult.GetActor() ? HitResult.GetActor()->GetName() : TEXT("None");
486 //UE_LOG(LogTemp, Log, TEXT("Drone is %f meters above ground. Hit object: %s"), DroneHeightFromGround, *HitActorName);
487 }
488 }
489}
490
491TArray<FTransform> APIDDrone::GenerateRoamingPoints(float radius, int32 roamingPoints)
492{
493 TArray<FTransform> generatedRoamingPoints;
494 generatedRoamingPoints.Reserve(roamingPoints);
495
496 FVector currentPosition = GetTransform().GetLocation();
497
498 FVector min = currentPosition - FVector(radius / 2, radius / 2, 0);
499 FVector max = currentPosition + FVector(radius / 2, radius / 2, 0);
500
501 for (int32 i = 0; i < roamingPoints; i++)
502 {
503 FTransform randomPoint;
504 randomPoint.SetLocation(FVector(FMath::RandRange(min.X, max.X), FMath::RandRange(min.Y, max.Y), 5000));
505
506 generatedRoamingPoints.Add(randomPoint);
507#if WITH_EDITOR
508 UE_LOG(LogTemp, Warning, TEXT("Waypoint %i: (%s)"), i, *randomPoint.GetLocation().ToString());
509#endif
510 }
511
512 return generatedRoamingPoints;
513}
514
516{
517 switch (ROSState)
518 {
520 CreateTopic();
521 break;
522
524 DestroyTopic();
525 break;
526 }
527}
528
530{
531 if (ROSTopic)
532 {
533 return;
534 }
535
536 UROSIntegrationGameInstance* ROSInstance = UAgrarsenseStatics::GetROSGameInstance(GetWorld());
537 if (ROSInstance && ROSInstance->IsROSConnected())
538 {
539 ROSTopic = NewObject<UTopic>(UTopic::StaticClass());
540 if (ROSTopic)
541 {
542
543 FString TopicName = FString::Printf(TEXT("/agrarsense/out/vehicles/%s/height"), *GetActorID_Implementation());
544 //UE_LOG(LogTemp, Warning, TEXT("TopicName is %s"), *TopicName);
545
546 ROSTopic->Init(ROSInstance->ROSIntegrationCore, TopicName, "std_msgs/Float32");
547 ROSTopic->Advertise();
548 }
549 }
550}
551
552float APIDDrone::GetYawRotationDifference(USkeletalMeshComponent* DroneMesh, UStaticMeshComponent* DesiredLocation)
553{
554 if (!DroneMesh || !DesiredLocation)
555 {
556 return 0.0f;
557 }
558
559 FVector ToTarget = DesiredLocation->GetComponentLocation() - DroneMesh->GetComponentLocation();
560 FRotator LookRotation = ToTarget.Rotation();
561
562 float CurrentYaw = DroneMesh->GetComponentRotation().Yaw;
563 float DesiredYaw = LookRotation.Yaw;
564
565 float DeltaYaw = FRotator::NormalizeAxis(DesiredYaw - CurrentYaw);
566 return DeltaYaw;
567}
568
570{
571 if (ROSTopic)
572 {
573 ROSTopic->Unadvertise();
574 ROSTopic->Unsubscribe();
575 ROSTopic->MarkAsDisconnected();
576 ROSTopic->ConditionalBeginDestroy();
577 ROSTopic = nullptr;
578 }
579}
EROSState
Definition: ROSState.h:16
void SetVisualizeOverlapArea(bool Visualize)
void SetOverlapBounds(const FVector &NewSize)
TSharedPtr< ROSMessages::std_msgs::Float32 > ROSMessage
Definition: PIDDrone.h:201
void CreateTopic()
Definition: PIDDrone.cpp:529
bool waypointReached
Definition: PIDDrone.h:272
TArray< FVector > WayPoints
Definition: PIDDrone.h:267
void AssignRoamingPoints(const TArray< FTransform > Points)
Definition: PIDDrone.cpp:396
void SetDroneRotation(USkeletalMeshComponent *target, FRotator rotator)
Definition: PIDDrone.cpp:406
FVector GetCurrentWaypointTarget()
Definition: PIDDrone.cpp:381
float DroneHeightFromGround
Definition: PIDDrone.h:282
void MoveDroneToPosition(const FTransform Transform)
Override all drone roaming points and continue towards this position.
Definition: PIDDrone.cpp:368
UWorld * World
Definition: PIDDrone.h:253
void AutoPilot(const float DeltaTime)
Definition: PIDDrone.cpp:228
int32 passedWaypoints
Definition: PIDDrone.h:280
FVector StartingPosition
Definition: PIDDrone.h:255
virtual void BeginPlay() override
Definition: PIDDrone.cpp:112
void UpdateGroundHeight()
Definition: PIDDrone.cpp:436
void HandleDestroy()
Definition: PIDDrone.cpp:268
void ClearWaypoints()
Definition: PIDDrone.h:118
UTopic * ROSTopic
Definition: PIDDrone.h:199
void SetFlightpath()
Called in tick function for drone roaming through points.
Definition: PIDDrone.cpp:288
void ROSBridgeStateChanged(EROSState ROSState) override
Definition: PIDDrone.cpp:515
float GetYawRotationDifference(USkeletalMeshComponent *DroneSkeletalMesh, UStaticMeshComponent *DesiredLocation)
Get wanted waypoint target rotation.
Definition: PIDDrone.cpp:552
virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override
Definition: PIDDrone.cpp:189
float distanceToNextPoint
Definition: PIDDrone.h:284
void DrawDebugPoints()
Definition: PIDDrone.cpp:238
void DestroyTopic()
Definition: PIDDrone.cpp:569
FTransform GetCurrentWaypointTarget_Transform()
Definition: PIDDrone.cpp:386
FDroneParameters DroneParameters
Definition: PIDDrone.h:247
bool IsRoaming() const
Definition: PIDDrone.h:230
AOverlapSensor * InnerOverlapSensor
Definition: PIDDrone.h:264
FDroneFinished OnDroneFinished
Definition: PIDDrone.h:61
TArray< FTransform > dronePath
Definition: PIDDrone.h:286
bool drawDebugPoints
Definition: PIDDrone.h:278
UStaticMeshComponent * PositionMesh
Definition: PIDDrone.h:261
USkeletalMeshComponent * DroneSkeletalMesh
Definition: PIDDrone.h:258
FTimerHandle DestroyHandle
Definition: PIDDrone.h:288
void ChangeDroneParameters(const FDroneParameters &newParameters)
Definition: PIDDrone.cpp:36
TArray< FTransform > GenerateRoamingPoints(float radius, int32 roamingPoints)
Generates a roadming points array for the drone in radius.
Definition: PIDDrone.cpp:491
virtual void Tick(float DeltaTime) override
Definition: PIDDrone.cpp:209
ACollisionSensor * CollisionSensor
Definition: Vehicle.h:290
virtual FString GetActorID_Implementation() const override
Definition: Vehicle.h:204
FText InteractableName
Definition: Vehicle.h:263
bool IsVehicleInGarage() const
Definition: Vehicle.h:133
FString ActorID
Definition: Vehicle.h:310
AOverlapSensor * OverlapSensor
Definition: Vehicle.h:296
ATransformSensor * TransformSensor
Definition: Vehicle.h:293
UNiagaraComponent * NiagaraComponent
Definition: Vehicle.h:299
static UROSIntegrationGameInstance * GetROSGameInstance(const UObject *WorldContextObject)
static ASimulationLevelManager * GetSimulationLevelManager(const UObject *WorldContextObject)
static ATransformSensor * SpawnTransformSensor(const FSensorSpawnParameters SpawnParameters, FTransformSensorParameters SensorParameters)
static ACollisionSensor * SpawnCollisionSensor(const FSensorSpawnParameters SpawnParameters, const FCollisionSensorParameters CollisionSensorParameters)
static AOverlapSensor * SpawnOverlapSensor(const FSensorSpawnParameters SpawnParameters, FOverlapSensorParameters SensorParameters)
UPrimitiveComponent * PrimitiveComponent
float InnerOverlapRadiusMeters
EDroneEndAction DroneEndAction
EDroneAction DroneAction
TArray< FTransform > Points
UPrimitiveComponent * PrimitiveComponent