19#include "ROSIntegration/Classes/RI/Topic.h"
21#include "Camera/CameraComponent.h"
22#include "Field/FieldSystemNodes.h"
23#include "NiagaraComponent.h"
24#include "Algo/Reverse.h"
27#include "DrawDebugHelpers.h"
32 PrimaryActorTick.bCanEverTick =
true;
33 InteractableName = NSLOCTEXT(
"Agrarsense",
"DroneInteractableName",
"Drone");
42 FString OverlapSensorID =
ActorID +
"/inner_overlap";
45 SpawnParams.
Transform = GetActorTransform();
95 FString VehicleCollisionSensorID =
ActorID +
"/collision";
97 CollisionSensorSpawnParams.
Transform = GetActorTransform();
99 CollisionSensorSpawnParams.
SensorName =
"collision";
101 CollisionSensorSpawnParams.
Parent =
this;
119 ROSMessage = MakeShared<ROSMessages::std_msgs::Float32>();
124 DroneSkeletalMesh = Cast<USkeletalMeshComponent>(GetComponentByClass(USkeletalMeshComponent::StaticClass()));
125 PositionMesh = Cast<UStaticMeshComponent>(GetComponentByClass(UStaticMeshComponent::StaticClass()));
128 FTransform ActorTransform = GetActorTransform();
134 FString VehicleTransformSensorID =
ActorID +
"/transform";
137 TransformSensorSpawnParams.
Transform = GetActorTransform();
139 TransformSensorSpawnParams.
SensorName =
"transform";
141 TransformSensorSpawnParams.
Parent =
this;
156 FString OverlapSensorID =
ActorID +
"/overlap";
159 OverlapSensorSpawnParams.
Transform = GetActorTransform();
161 OverlapSensorSpawnParams.
SensorName =
"overlap";
163 OverlapSensorSpawnParams.
Parent =
this;
168 OverlapSensorParams.
Size = FVector(25.0f, 25.0f, 25.0f);
191 Super::EndPlay(EndPlayReason);
211 Super::Tick(DeltaTime);
244 for (int32 i = 0; i < dronePoints.Num(); i++)
249 DrawDebugSphere(
World, dronePoints[i].GetLocation(), 5.0f, 25, FColor::Red,
false);
250 if (i < dronePoints.Num() - 1)
252 FVector nextPointLocation = dronePoints[i + 1].GetLocation();
253 DrawDebugLine(
World, dronePoints[i].GetLocation(), nextPointLocation, FColor::Blue,
false, 0.0f, 0, 5.0f);
258 for (int32 i = 0; i <
dronePath.Num(); i++)
262 FVector nextPointLocation =
dronePath[i + 1].GetLocation();
263 DrawDebugLine(
World,
dronePath[i].GetLocation(), nextPointLocation, FColor::Green,
false, 0.0f, 0, 5.0f);
273 if (SimulationLevelManager)
277 if (controlledVehicle ==
this)
301 FVector currentlocation =
DroneSkeletalMesh->GetRelativeTransform().GetLocation();
311 UE_LOG(LogTemp, Warning, TEXT(
"Desired location mesh not found"));
315 distanceToNextPoint = FVector2f::Distance(FVector2f(waypoint.X, waypoint.Y), FVector2f(currentlocation.X, currentlocation.Y));
355 if (!GetWorld()->GetTimerManager().IsTimerActive(
DestroyHandle))
401 for (
const FTransform& point : Points)
414 FRotator currentRotation = target->GetRelativeRotation();
415 FRotator rotationDifference = rotator - currentRotation;
417 target->AddRelativeRotation(rotationDifference,
false,
nullptr, ETeleportType::TeleportPhysics);
420 if (FMath::Abs(rotator.Pitch) < 0.001f)
422 rotator.Pitch = 0.0f;
424 if (FMath::Abs(rotator.Yaw) < 0.001f)
428 if (FMath::Abs(rotator.Roll) < 0.001f)
434 target->SetRelativeRotation(rotator,
false,
nullptr, ETeleportType::TeleportPhysics);
445 FVector End = Start - FVector(0, 0, 10000.0f);
447 FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT(
"UpdateGroundHeight")),
true,
this);
448 TraceParams.bReturnPhysicalMaterial =
false;
449 TraceParams.bTraceComplex =
false;
452 TArray<AActor*> AttachedActors;
453 GetAttachedActors(AttachedActors);
454 for (AActor* AttachedActor : AttachedActors)
457 if (AttachedActor && Cast<ASensor>(AttachedActor))
459 TArray<AActor*> ChildActors;
460 AttachedActor->GetAttachedActors(ChildActors);
461 AttachedActors.Append(ChildActors);
464 TraceParams.AddIgnoredActors(AttachedActors);
466 FHitResult HitResult;
468#ifdef ParallelLineTraceSingleByChannel_EXISTS
471 World->ParallelLineTraceSingleByChannel(
477 FCollisionResponseParams::DefaultResponseParam);
480 World->LineTraceSingleByChannel(
486 FCollisionResponseParams::DefaultResponseParam);
489 if (HitResult.IsValidBlockingHit())
491 float Height = (Start.Z - HitResult.ImpactPoint.Z) / 100.0f;
492 float HeightRounded = FMath::RoundToFloat(Height * 1000.0f) / 1000.0f;
509 TArray<FTransform> generatedRoamingPoints;
510 generatedRoamingPoints.Reserve(roamingPoints);
512 FVector currentPosition = GetTransform().GetLocation();
514 FVector min = currentPosition - FVector(radius / 2, radius / 2, 0);
515 FVector max = currentPosition + FVector(radius / 2, radius / 2, 0);
517 for (int32 i = 0; i < roamingPoints; i++)
519 FTransform randomPoint;
520 randomPoint.SetLocation(FVector(FMath::RandRange(min.X, max.X), FMath::RandRange(min.Y, max.Y), 5000));
522 generatedRoamingPoints.Add(randomPoint);
524 UE_LOG(LogTemp, Warning, TEXT(
"Waypoint %i: (%s)"), i, *randomPoint.GetLocation().ToString());
528 return generatedRoamingPoints;
553 if (ROSInstance && ROSInstance->IsROSConnected())
555 ROSTopic = NewObject<UTopic>(UTopic::StaticClass());
562 ROSTopic->Init(ROSInstance->ROSIntegrationCore, TopicName,
"std_msgs/Float32");
570 if (!DroneMesh || !DesiredLocation)
575 FVector ToTarget = DesiredLocation->GetComponentLocation() - DroneMesh->GetComponentLocation();
576 FRotator LookRotation = ToTarget.Rotation();
578 float CurrentYaw = DroneMesh->GetComponentRotation().Yaw;
579 float DesiredYaw = LookRotation.Yaw;
581 float DeltaYaw = FRotator::NormalizeAxis(DesiredYaw - CurrentYaw);
592 ROSTopic->ConditionalBeginDestroy();
void SetVisualizeOverlapArea(bool Visualize)
void SetOverlapBounds(const FVector &NewSize)
TSharedPtr< ROSMessages::std_msgs::Float32 > ROSMessage
TArray< FVector > WayPoints
void AssignRoamingPoints(const TArray< FTransform > Points)
void SetDroneRotation(USkeletalMeshComponent *target, FRotator rotator)
FVector GetCurrentWaypointTarget()
float DroneHeightFromGround
void MoveDroneToPosition(const FTransform Transform)
Override all drone roaming points and continue towards this position.
void AutoPilot(const float DeltaTime)
virtual void BeginPlay() override
void UpdateGroundHeight()
void SetFlightpath()
Called in tick function for drone roaming through points.
void ROSBridgeStateChanged(EROSState ROSState) override
float GetYawRotationDifference(USkeletalMeshComponent *DroneSkeletalMesh, UStaticMeshComponent *DesiredLocation)
Get wanted waypoint target rotation.
virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override
float distanceToNextPoint
FTransform GetCurrentWaypointTarget_Transform()
FDroneParameters DroneParameters
AOverlapSensor * InnerOverlapSensor
FDroneFinished OnDroneFinished
TArray< FTransform > dronePath
UStaticMeshComponent * PositionMesh
USkeletalMeshComponent * DroneSkeletalMesh
FTimerHandle DestroyHandle
void ChangeDroneParameters(const FDroneParameters &newParameters)
TArray< FTransform > GenerateRoamingPoints(float radius, int32 roamingPoints)
Generates a roadming points array for the drone in radius.
virtual void Tick(float DeltaTime) override
void CeaseManualControlOfVehicle()
AVehicle * GetManuallyControlledVehicle()
ACollisionSensor * CollisionSensor
virtual FString GetActorID_Implementation() const override
bool IsVehicleInGarage() const
AOverlapSensor * OverlapSensor
ATransformSensor * TransformSensor
UNiagaraComponent * NiagaraComponent
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
bool CreateInnerOverlapSensor
EDroneEndAction DroneEndAction
float OverlapRadiusMeters
TArray< FTransform > Points