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)
300 FVector currentlocation =
DroneSkeletalMesh->GetRelativeTransform().GetLocation();
310 UE_LOG(LogTemp, Warning, TEXT(
"Desired location mesh not found"));
314 distanceToNextPoint = FVector2f::Distance(FVector2f(waypoint.X, waypoint.Y), FVector2f(currentlocation.X, currentlocation.Y));
354 if (!GetWorld()->GetTimerManager().IsTimerActive(
DestroyHandle))
400 for (
const FTransform& point : Points)
413 FRotator currentRotation = target->GetRelativeRotation();
414 FRotator rotationDifference = rotator - currentRotation;
416 target->AddRelativeRotation(rotationDifference,
false,
nullptr, ETeleportType::TeleportPhysics);
419 if (FMath::Abs(rotator.Pitch) < 0.001f)
421 rotator.Pitch = 0.0f;
423 if (FMath::Abs(rotator.Yaw) < 0.001f)
427 if (FMath::Abs(rotator.Roll) < 0.001f)
433 target->SetRelativeRotation(rotator,
false,
nullptr, ETeleportType::TeleportPhysics);
444 FVector End = Start - FVector(0, 0, 10000.0f);
446 FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT(
"UpdateGroundHeight")),
true,
this);
447 TraceParams.bReturnPhysicalMaterial =
false;
448 TraceParams.bTraceComplex =
false;
450 FHitResult HitResult;
452#ifdef ParallelLineTraceSingleByChannel_EXISTS
455 World->ParallelLineTraceSingleByChannel(
461 FCollisionResponseParams::DefaultResponseParam);
464 World->LineTraceSingleByChannel(
470 FCollisionResponseParams::DefaultResponseParam);
473 if (HitResult.IsValidBlockingHit())
475 float Height = (Start.Z - HitResult.ImpactPoint.Z) / 100.0f;
476 float HeightRounded = FMath::RoundToFloat(Height * 1000.0f) / 1000.0f;
493 TArray<FTransform> generatedRoamingPoints;
494 generatedRoamingPoints.Reserve(roamingPoints);
496 FVector currentPosition = GetTransform().GetLocation();
498 FVector min = currentPosition - FVector(radius / 2, radius / 2, 0);
499 FVector max = currentPosition + FVector(radius / 2, radius / 2, 0);
501 for (int32 i = 0; i < roamingPoints; i++)
503 FTransform randomPoint;
504 randomPoint.SetLocation(FVector(FMath::RandRange(min.X, max.X), FMath::RandRange(min.Y, max.Y), 5000));
506 generatedRoamingPoints.Add(randomPoint);
508 UE_LOG(LogTemp, Warning, TEXT(
"Waypoint %i: (%s)"), i, *randomPoint.GetLocation().ToString());
512 return generatedRoamingPoints;
537 if (ROSInstance && ROSInstance->IsROSConnected())
539 ROSTopic = NewObject<UTopic>(UTopic::StaticClass());
546 ROSTopic->Init(ROSInstance->ROSIntegrationCore, TopicName,
"std_msgs/Float32");
554 if (!DroneMesh || !DesiredLocation)
559 FVector ToTarget = DesiredLocation->GetComponentLocation() - DroneMesh->GetComponentLocation();
560 FRotator LookRotation = ToTarget.Rotation();
562 float CurrentYaw = DroneMesh->GetComponentRotation().Yaw;
563 float DesiredYaw = LookRotation.Yaw;
565 float DeltaYaw = FRotator::NormalizeAxis(DesiredYaw - CurrentYaw);
576 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