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 // Check if drone is in garage
292 return;
293
294 // Don't try to fly if no waypoints
295 if (DroneParameters.Points.Num() == 0 && IsRoaming())
296 {
298 }
299
300 FVector waypoint = GetCurrentWaypointTarget();
301 FVector currentlocation = DroneSkeletalMesh->GetRelativeTransform().GetLocation();
302
303 if (PositionMesh)
304 {
305 // Doesn't work
306 PositionMesh->SetWorldLocation(waypoint);
307 }
308#if WITH_EDITOR
309 else
310 {
311 UE_LOG(LogTemp, Warning, TEXT("Desired location mesh not found"));
312 //DrawDebugLine(World, currentlocation, waypoint, FColor::Red, false, 0.0f, 0.0f, 5.0f);
313 }
314#endif
315 distanceToNextPoint = FVector2f::Distance(FVector2f(waypoint.X, waypoint.Y), FVector2f(currentlocation.X, currentlocation.Y));
316 if (distanceToNextPoint < 100)
317 {
318 waypointReached = true;
319 if (passedWaypoints != DroneParameters.Points.Num() - 1)
320 {
321#if WITH_EDITOR
322 UE_LOG(LogTemp, Warning, TEXT("Changing waypoint from %i to %i"), passedWaypoints, passedWaypoints + 1);
323#endif
324
326 }
327 else
328 {
330 {
332 passedWaypoints = 0;
333 }
334 else
335 {
337 {
340 OnDroneFinished.Broadcast(DroneParameters.Points.Last());
341 break;
344 passedWaypoints = 0;
345 break;
347 passedWaypoints = 0;
348 break;
350 Algo::Reverse(DroneParameters.Points);
351 passedWaypoints = 0;
352 break;
354 OnDroneFinished.Broadcast(DroneParameters.Points.Last());
355 if (!GetWorld()->GetTimerManager().IsTimerActive(DestroyHandle))
356 {
358 GetWorld()->GetTimerManager().SetTimer(DestroyHandle, this, &APIDDrone::HandleDestroy, 5.0f, false);
359 }
360 break;
361 default:
362 break;
363 }
364 }
365 }
366 }
367}
368
369void APIDDrone::MoveDroneToPosition(const FTransform Transform)
370{
371 if (DroneParameters.Points.Num() > 0)
372 {
373 DroneParameters.Points.Empty();
375 }
376 else
377 {
379 }
380}
381
383{
384 return GetCurrentWaypointTarget_Transform().GetLocation();
385}
386
388{
390 {
391 return DroneSkeletalMesh->GetRelativeTransform();
392 }
393
395}
396
397void APIDDrone::AssignRoamingPoints(const TArray<FTransform> Points)
398{
400
401 for (const FTransform& point : Points)
402 {
403 WayPoints.Add(point.GetLocation());
404 }
405}
406
407void APIDDrone::SetDroneRotation(USkeletalMeshComponent* target, FRotator rotator)
408{
409 if (!target)
410 {
411 return;
412 }
413
414 FRotator currentRotation = target->GetRelativeRotation();
415 FRotator rotationDifference = rotator - currentRotation;
416
417 target->AddRelativeRotation(rotationDifference, false, nullptr, ETeleportType::TeleportPhysics);
418
419 // Prevent locking up when using interp
420 if (FMath::Abs(rotator.Pitch) < 0.001f)
421 {
422 rotator.Pitch = 0.0f;
423 }
424 if (FMath::Abs(rotator.Yaw) < 0.001f)
425 {
426 rotator.Yaw = 0.0f;
427 }
428 if (FMath::Abs(rotator.Roll) < 0.001f)
429 {
430 rotator.Roll = 0.0f;
431 }
432
433 // Corrected rotation
434 target->SetRelativeRotation(rotator, false, nullptr, ETeleportType::TeleportPhysics);
435}
436
438{
439 if (!DroneSkeletalMesh || !ROSTopic || !ROSMessage.IsValid())
440 {
441 return;
442 }
443
444 FVector Start = DroneSkeletalMesh->GetComponentLocation();
445 FVector End = Start - FVector(0, 0, 10000.0f);
446
447 FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("UpdateGroundHeight")), true, this);
448 TraceParams.bReturnPhysicalMaterial = false;
449 TraceParams.bTraceComplex = false;
450
451 // Get all child actors and add them to ignored actors list.
452 TArray<AActor*> AttachedActors;
453 GetAttachedActors(AttachedActors);
454 for (AActor* AttachedActor : AttachedActors)
455 {
456 // If the attached actor is a sensor, also ignore its children (e.g., sensor model mesh).
457 if (AttachedActor && Cast<ASensor>(AttachedActor))
458 {
459 TArray<AActor*> ChildActors;
460 AttachedActor->GetAttachedActors(ChildActors);
461 AttachedActors.Append(ChildActors);
462 }
463 }
464 TraceParams.AddIgnoredActors(AttachedActors);
465
466 FHitResult HitResult;
467
468#ifdef ParallelLineTraceSingleByChannel_EXISTS
469 // Defined and implemented in our AGRARSENSE Unreal Engine fork
470 // This LineTrace method doesn't block the physics scene which improves linetrace performance.
471 World->ParallelLineTraceSingleByChannel(
472 HitResult,
473 Start,
474 End,
475 ECC_Visibility,
476 TraceParams,
477 FCollisionResponseParams::DefaultResponseParam);
478#else
479 // If not using our fork of the engine, use default LineTrace method.
480 World->LineTraceSingleByChannel(
481 HitResult,
482 Start,
483 End,
484 ECC_Visibility,
485 TraceParams,
486 FCollisionResponseParams::DefaultResponseParam);
487#endif
488
489 if (HitResult.IsValidBlockingHit())
490 {
491 float Height = (Start.Z - HitResult.ImpactPoint.Z) / 100.0f;
492 float HeightRounded = FMath::RoundToFloat(Height * 1000.0f) / 1000.0f;
493
494 if (!FMath::IsNearlyEqual(DroneHeightFromGround, HeightRounded, 0.005f))
495 {
496 DroneHeightFromGround = HeightRounded;
497
499 ROSTopic->Publish(ROSMessage);
500
501 //FString HitActorName = HitResult.GetActor() ? HitResult.GetActor()->GetName() : TEXT("None");
502 //UE_LOG(LogTemp, Log, TEXT("Drone is %f meters above ground. Hit object: %s"), DroneHeightFromGround, *HitActorName);
503 }
504 }
505}
506
507TArray<FTransform> APIDDrone::GenerateRoamingPoints(float radius, int32 roamingPoints)
508{
509 TArray<FTransform> generatedRoamingPoints;
510 generatedRoamingPoints.Reserve(roamingPoints);
511
512 FVector currentPosition = GetTransform().GetLocation();
513
514 FVector min = currentPosition - FVector(radius / 2, radius / 2, 0);
515 FVector max = currentPosition + FVector(radius / 2, radius / 2, 0);
516
517 for (int32 i = 0; i < roamingPoints; i++)
518 {
519 FTransform randomPoint;
520 randomPoint.SetLocation(FVector(FMath::RandRange(min.X, max.X), FMath::RandRange(min.Y, max.Y), 5000));
521
522 generatedRoamingPoints.Add(randomPoint);
523#if WITH_EDITOR
524 UE_LOG(LogTemp, Warning, TEXT("Waypoint %i: (%s)"), i, *randomPoint.GetLocation().ToString());
525#endif
526 }
527
528 return generatedRoamingPoints;
529}
530
532{
533 switch (ROSState)
534 {
536 CreateTopic();
537 break;
538
540 DestroyTopic();
541 break;
542 }
543}
544
546{
547 if (ROSTopic)
548 {
549 return;
550 }
551
552 UROSIntegrationGameInstance* ROSInstance = UAgrarsenseStatics::GetROSGameInstance(GetWorld());
553 if (ROSInstance && ROSInstance->IsROSConnected())
554 {
555 ROSTopic = NewObject<UTopic>(UTopic::StaticClass());
556 if (ROSTopic)
557 {
558
559 FString TopicName = FString::Printf(TEXT("/agrarsense/out/vehicles/%s/height"), *GetActorID_Implementation());
560 //UE_LOG(LogTemp, Warning, TEXT("TopicName is %s"), *TopicName);
561
562 ROSTopic->Init(ROSInstance->ROSIntegrationCore, TopicName, "std_msgs/Float32");
563 ROSTopic->Advertise();
564 }
565 }
566}
567
568float APIDDrone::GetYawRotationDifference(USkeletalMeshComponent* DroneMesh, UStaticMeshComponent* DesiredLocation)
569{
570 if (!DroneMesh || !DesiredLocation)
571 {
572 return 0.0f;
573 }
574
575 FVector ToTarget = DesiredLocation->GetComponentLocation() - DroneMesh->GetComponentLocation();
576 FRotator LookRotation = ToTarget.Rotation();
577
578 float CurrentYaw = DroneMesh->GetComponentRotation().Yaw;
579 float DesiredYaw = LookRotation.Yaw;
580
581 float DeltaYaw = FRotator::NormalizeAxis(DesiredYaw - CurrentYaw);
582 return DeltaYaw;
583}
584
586{
587 if (ROSTopic)
588 {
589 ROSTopic->Unadvertise();
590 ROSTopic->Unsubscribe();
591 ROSTopic->MarkAsDisconnected();
592 ROSTopic->ConditionalBeginDestroy();
593 ROSTopic = nullptr;
594 }
595}
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:545
bool waypointReached
Definition: PIDDrone.h:272
TArray< FVector > WayPoints
Definition: PIDDrone.h:267
void AssignRoamingPoints(const TArray< FTransform > Points)
Definition: PIDDrone.cpp:397
void SetDroneRotation(USkeletalMeshComponent *target, FRotator rotator)
Definition: PIDDrone.cpp:407
FVector GetCurrentWaypointTarget()
Definition: PIDDrone.cpp:382
float DroneHeightFromGround
Definition: PIDDrone.h:282
void MoveDroneToPosition(const FTransform Transform)
Override all drone roaming points and continue towards this position.
Definition: PIDDrone.cpp:369
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:437
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:531
float GetYawRotationDifference(USkeletalMeshComponent *DroneSkeletalMesh, UStaticMeshComponent *DesiredLocation)
Get wanted waypoint target rotation.
Definition: PIDDrone.cpp:568
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:585
FTransform GetCurrentWaypointTarget_Transform()
Definition: PIDDrone.cpp:387
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:507
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