PIDDrone.cpp

Go to the documentation of this file.

// Copyright (c) 2023 FrostBit Software Lab at the Lapland University of Applied Sciences
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
 
#include "PIDDrone.h"
 
#include "Agrarsense/Game/AgrarsenseGameInstance.h"
#include "Agrarsense/Game/AgrarsenseStatics.h"
#include "Agrarsense/Managers/SimulationLevelManager.h"
#include "Agrarsense/ROS/ROSHandler.h"
 
#include "Agrarsense/Sensor/Transform/TransformSensorParameters.h"
#include "Agrarsense/Sensor/Transform/TransformSensor.h"
#include "Agrarsense/Sensor/Overlap/OverlapSensor.h"
#include "Agrarsense/Sensor/SensorFactory.h"
#include "ROSIntegration/Classes/RI/Topic.h"
 
#include "Camera/CameraComponent.h"
#include "Field/FieldSystemNodes.h"
#include "NiagaraComponent.h"
#include "Algo/Reverse.h"
 
#if WITH_EDITOR
#include "DrawDebugHelpers.h"
#endif
 
APIDDrone::APIDDrone() : Super()
{
    PrimaryActorTick.bCanEverTick = true;
    InteractableName = NSLOCTEXT("Agrarsense", "DroneInteractableName", "Drone");
}
 
void APIDDrone::ChangeDroneParameters(const FDroneParameters& newParameters)
{
#if WITH_EDITOR
    UE_LOG(LogTemp, Warning, TEXT("Parameters Changed!"));
#endif
 
    DroneParameters = newParameters;
 
    if (DroneParameters.CreateInnerOverlapSensor && !InnerOverlapSensor)
    {
        // Create OverlapSensor
        FOverlapSensorParameters Params;
        Params.OwningActor = this;
        Params.AllChannels = true;
 
        const float RadiusMeters = DroneParameters.InnerOverlapRadiusMeters;
        Params.Size = FVector(RadiusMeters, RadiusMeters, RadiusMeters);
 
        Params.RelativePosition = FVector(0.0f, 0.0f, 0.0f);
        FString OverlapSensorID = ActorID + "/inner_overlap";
        InnerOverlapSensor = USensorFactory::SpawnOverlapSensor(GetActorTransform(), Params, OverlapSensorID, "overlap", this);
 
        if (InnerOverlapSensor)
        {
            InnerOverlapSensor->AttachToComponent(DroneSkeletalMesh, FAttachmentTransformRules::KeepRelativeTransform);
        }
    }
    else if (InnerOverlapSensor && !DroneParameters.CreateInnerOverlapSensor)
    {
        InnerOverlapSensor->Destroy();
        InnerOverlapSensor = nullptr;
    }
 
    if (!DroneParameters.CollisionsEnabled && DroneSkeletalMesh)
    {
        DroneSkeletalMesh->SetCollisionProfileName(TEXT("DroneNoCollision"));
    }
 
    if (OverlapSensor)
    {
        OverlapSensor->SetVisualizeOverlapArea(DroneParameters.VisualizeOverlap);
 
        const float BoundsSizeMeters = DroneParameters.OverlapRadiusMeters;
        OverlapSensor->SetOverlapBounds(FVector(BoundsSizeMeters, BoundsSizeMeters, BoundsSizeMeters));
 
    }
 
    if (InnerOverlapSensor)
    {
        InnerOverlapSensor->SetVisualizeOverlapArea(DroneParameters.VisualizeOverlap);
 
        const float BoundsSizeMeters = DroneParameters.InnerOverlapRadiusMeters;
        InnerOverlapSensor->SetOverlapBounds(FVector(BoundsSizeMeters, BoundsSizeMeters, BoundsSizeMeters));
    }
}
 
void APIDDrone::BeginPlay()
{
    Super::BeginPlay();
 
    World = GetWorld();
 
    ROSMessage.Reset();
    ROSMessage = MakeShared<ROSMessages::std_msgs::Float32>();
 
    CreateTopic();
 
    // These should be setup in BP_Drone_PID blueprint.
    DroneSkeletalMesh = Cast<USkeletalMeshComponent>(GetComponentByClass(USkeletalMeshComponent::StaticClass()));
    PositionMesh = Cast<UStaticMeshComponent>(GetComponentByClass(UStaticMeshComponent::StaticClass()));
 
    StartingPosition = GetActorLocation();
    FTransform ActorTransform = GetActorTransform();
 
    if (DroneSkeletalMesh && !TransformSensor)
    {
        // Create Transform sensor for forwarder and harvester
        FTransformSensorParameters Params;
        Params.SaveTransformDataToDisk = true;
        Params.OwningActor = this;
        Params.UseOwningActorTransform = false;
        Params.PrimitiveComponent = Cast<UPrimitiveComponent>(DroneSkeletalMesh);
 
        FString VehicleTransformSensorID = ActorID + "/transform";
        TransformSensor = USensorFactory::SpawnTransformSensor(ActorTransform, Params, VehicleTransformSensorID, "transform", true, this);
    }
 
    if (!OverlapSensor)
    {
        // Create OverlapSensor
        FOverlapSensorParameters Params;
        Params.OwningActor = this;
        Params.Size = FVector(25.0f, 25.0f, 25.0f);
        Params.RelativePosition = FVector(0.0f, 0.0f, 0.0f);
        FString OverlapSensorID = ActorID + "/overlap";
        OverlapSensor = USensorFactory::SpawnOverlapSensor(ActorTransform, Params, OverlapSensorID, "overlap", this);
 
        if (OverlapSensor) 
        {
            OverlapSensor->AttachToComponent(DroneSkeletalMesh, FAttachmentTransformRules::KeepRelativeTransform);
        }
    }
 
    if (NiagaraComponent)
    {
        // Attach rain/snowfall niagara component to drone skeletal mesh.
        NiagaraComponent->AttachToComponent(DroneSkeletalMesh, FAttachmentTransformRules::KeepRelativeTransform);
    }
 
    // Apply default parameters
    ChangeDroneParameters(DroneParameters);
}
 
void APIDDrone::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
 
    if (TransformSensor)
    {
        TransformSensor->Destroy();
        TransformSensor = nullptr;
    }
 
    if (InnerOverlapSensor) 
    {
        InnerOverlapSensor->Destroy();
        InnerOverlapSensor = nullptr;
    }
 
    ROSMessage.Reset();
    DestroyTopic();
}
 
void APIDDrone::Tick(float DeltaTime)
{
    Super::Tick(DeltaTime);
 
    if (!IsVehicleInGarage())
    {
        AutoPilot(DeltaTime);
        UpdateGroundHeight();
 
#if WITH_EDITOR
        if (drawDebugPoints)
        {
            DrawDebugPoints();
        }
#endif
 
    }
}
 
void APIDDrone::AutoPilot(const float DeltaTime)
{
    if (DroneParameters.DroneAction == EDroneAction::Stationary)
    {
        return;
    }
 
    SetFlightpath();
}
 
void APIDDrone::DrawDebugPoints()
{
    TArray<FTransform> dronePoints = DroneParameters.Points;
 
    dronePath.Add(DroneSkeletalMesh->GetComponentTransform());
 
    for (int i = 0; i < dronePoints.Num(); i++)
    {
        if (World)
        {
            //UE_LOG(LogTemp, Warning, TEXT("Drone point %d: %s"), i, *dronePoints[i].GetLocation().ToString());
            DrawDebugSphere(World, dronePoints[i].GetLocation(), 5.0f, 25, FColor::Red, false);
            if (i < dronePoints.Num() - 1)
            {
                FVector nextPointLocation = dronePoints[i + 1].GetLocation();
                DrawDebugLine(World, dronePoints[i].GetLocation(), nextPointLocation, FColor::Blue, false, 0.0f, 0, 5.0f);
            }
        }
    }
 
    for (int i = 0; i < dronePath.Num(); i++)
    {
        if (i < dronePath.Num() - 1)
        {
            FVector nextPointLocation = dronePath[i + 1].GetLocation();
            DrawDebugLine(World, dronePath[i].GetLocation(), nextPointLocation, FColor::Green, false, 0.0f, 0, 5.0f);
        }
    }
}
 
void APIDDrone::HandleDestroy()
{
    // Should trigger EndPlay?
 
    ASimulationLevelManager* SimulationLevelManager = UAgrarsenseStatics::GetSimulationLevelManager(World);
    if (SimulationLevelManager)
    {
        AVehicle* controlledVehicle = SimulationLevelManager->GetManuallyControlledVehicle();
 
        if (controlledVehicle == this) 
        {
            SimulationLevelManager->CeaseManualControlOfVehicle();
        }   
    }
 
    passedWaypoints = 0;
    Destroy();
}
 
 
void APIDDrone::SetFlightpath()
{
    // TODO: Check if in garage
    // include drone rotation to point
 
    // Don't try to fly if no waypoints
    if (DroneParameters.Points.Num() == 0 && IsRoaming())
    {
        DroneParameters.Points = GenerateRoamingPoints(50000, 10);
    }
 
    FVector waypoint = GetCurrentWaypointTarget();
    FVector currentlocation = DroneSkeletalMesh->GetRelativeTransform().GetLocation();
 
    if (PositionMesh)
    {
        // Doesn't work
        PositionMesh->SetWorldLocation(waypoint);
    }
#if WITH_EDITOR
    else
    {
        UE_LOG(LogTemp, Warning, TEXT("Desired location mesh not found"));
        //DrawDebugLine(World, currentlocation, waypoint, FColor::Red, false, 0.0f, 0.0f, 5.0f);
    }
#endif
    distanceToNextPoint = FVector2f::Distance(FVector2f(waypoint.X, waypoint.Y), FVector2f(currentlocation.X, currentlocation.Y));
    if (distanceToNextPoint < 100)
    {
        waypointReached = true;
        if (passedWaypoints != DroneParameters.Points.Num() - 1)
        {
#if WITH_EDITOR
            UE_LOG(LogTemp, Warning, TEXT("Changing waypoint from %i to %i"), passedWaypoints, passedWaypoints + 1);
#endif
 
            passedWaypoints++;
        }
        else
        {
            if (DroneParameters.DroneAction == EDroneAction::Roaming)
            {
                DroneParameters.Points = GenerateRoamingPoints(50000, 10);
                passedWaypoints = 0;
            }
            else
            {
                switch (DroneParameters.DroneEndAction)
                {
                case EDroneEndAction::Stop:
                    DroneParameters.DroneAction = EDroneAction::Stationary;
                    OnDroneFinished.Broadcast(DroneParameters.Points.Last());
                    break;
                case EDroneEndAction::GenerateRandomNew:
                    DroneParameters.Points = GenerateRoamingPoints(50000, 10);
                    passedWaypoints = 0;
                    break;
                case EDroneEndAction::RepeatFromBeginning:
                    passedWaypoints = 0;
                    break;
                case EDroneEndAction::GoBackwards:
                    Algo::Reverse(DroneParameters.Points);
                    passedWaypoints = 0;
                    break;
                case EDroneEndAction::Destroy:
                    OnDroneFinished.Broadcast(DroneParameters.Points.Last());
                    if (!GetWorld()->GetTimerManager().IsTimerActive(DestroyHandle))
                    {
                        DroneParameters.DroneAction = EDroneAction::Stationary;
                        GetWorld()->GetTimerManager().SetTimer(DestroyHandle, this, &APIDDrone::HandleDestroy, 5.0f, false);
                    }
                    break;
                default:
                    break;
                }
            }
        }
    }
}
 
void APIDDrone::MoveDroneToPosition(const FTransform Transform)
{
    if (DroneParameters.Points.Num() > 0)
    {
        DroneParameters.Points.Empty();
        DroneParameters.Points.Add(Transform);
    }
    else
    {
        DroneParameters.Points.Add(Transform);
    }
}
 
FVector APIDDrone::GetCurrentWaypointTarget()
{
    return GetCurrentWaypointTarget_Transform().GetLocation();
}
 
FTransform APIDDrone::GetCurrentWaypointTarget_Transform()
{
    if (DroneParameters.Points.Num() <= passedWaypoints)
    {
        return DroneSkeletalMesh->GetRelativeTransform();
    }
 
    return DroneParameters.Points[passedWaypoints];
}
 
void APIDDrone::AssignRoamingPoints(const TArray<FTransform> Points)
{
    ClearWaypoints();
 
    for (const FTransform& point : Points)
    {
        WayPoints.Add(point.GetLocation());
    }
}
 
void APIDDrone::SetDroneRotation(USkeletalMeshComponent* target, FRotator rotator)
{
    if (!target)
    {
        return;
    }
 
    FRotator currentRotation = target->GetRelativeRotation();
    FRotator rotationDifference = rotator - currentRotation;
 
    target->AddRelativeRotation(rotationDifference, false, nullptr, ETeleportType::TeleportPhysics);
 
    // Prevent locking up when using interp
    if (FMath::Abs(rotator.Pitch) < 0.001f)
    {
        rotator.Pitch = 0.0f;
    }
    if (FMath::Abs(rotator.Yaw) < 0.001f)
    {
        rotator.Yaw = 0.0f;
    }
    if (FMath::Abs(rotator.Roll) < 0.001f)
    {
        rotator.Roll = 0.0f;
    }
 
    // Corrected rotation
    target->SetRelativeRotation(rotator, false, nullptr, ETeleportType::TeleportPhysics);
}
 
void APIDDrone::UpdateGroundHeight()
{
    if (!DroneSkeletalMesh || !ROSTopic || !ROSMessage.IsValid())
    {
        return;
    }
 
    FVector Start = DroneSkeletalMesh->GetComponentLocation();
    FVector End = Start - FVector(0, 0, 10000.0f);
 
    FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("UpdateGroundHeight")), true, this);
    TraceParams.bReturnPhysicalMaterial = false;
    TraceParams.bTraceComplex = false;
 
    FHitResult HitResult;
 
#ifdef ParallelLineTraceSingleByChannel_EXISTS
    // Defined and implemented in our AGRARSENSE Unreal Engine fork
    // This LineTrace method doesn't block the physics scene which improves linetrace performance.
    World->ParallelLineTraceSingleByChannel(
        HitResult,
        Start,
        End,
        ECC_Visibility,
        TraceParams,
        FCollisionResponseParams::DefaultResponseParam);
#else
    // If not using our fork of the engine, use default LineTrace method.
    World->LineTraceSingleByChannel(
        HitResult,
        Start,
        End,
        ECC_Visibility,
        TraceParams,
        FCollisionResponseParams::DefaultResponseParam);
#endif
 
    if (HitResult.IsValidBlockingHit())
    {
        float Height = (Start.Z - HitResult.ImpactPoint.Z) / 100.0f;
        float HeightRounded = FMath::RoundToFloat(Height * 1000.0f) / 1000.0f;
 
        if (!FMath::IsNearlyEqual(DroneHeightFromGround, HeightRounded, 0.005f))
        {
            DroneHeightFromGround = HeightRounded;
 
            ROSMessage->_Data = DroneHeightFromGround;
            ROSTopic->Publish(ROSMessage);
 
            //FString HitActorName = HitResult.GetActor() ? HitResult.GetActor()->GetName() : TEXT("None");
            //UE_LOG(LogTemp, Log, TEXT("Drone is %f meters above ground. Hit object: %s"), DroneHeightFromGround, *HitActorName);
        }
    }
}
 
TArray<FTransform> APIDDrone::GenerateRoamingPoints(float radius, int32 roamingPoints)
{
    TArray<FTransform> generatedRoamingPoints;
    generatedRoamingPoints.Reserve(roamingPoints);
 
    FVector currentPosition = GetTransform().GetLocation();
 
    FVector min = currentPosition - FVector(radius / 2, radius / 2, 0);
    FVector max = currentPosition + FVector(radius / 2, radius / 2, 0);
 
    for (int32 i = 0; i < roamingPoints; i++)
    {
        FTransform randomPoint;
        randomPoint.SetLocation(FVector(FMath::RandRange(min.X, max.X), FMath::RandRange(min.Y, max.Y), 5000));
 
        generatedRoamingPoints.Add(randomPoint);
#if WITH_EDITOR
        UE_LOG(LogTemp, Warning, TEXT("Waypoint %i: (%s)"), i, *randomPoint.GetLocation().ToString());
#endif
    }
 
    return generatedRoamingPoints;
}
 
void APIDDrone::ROSBridgeStateChanged(EROSState ROSState)
{
    switch (ROSState)
    {
    case EROSState::Connected:
        CreateTopic();
        break;
 
    case EROSState::Disconnected:
        DestroyTopic();
        break;
    }
}
 
void APIDDrone::CreateTopic()
{
    if (ROSTopic)
    {
        return;
    }
 
    UROSIntegrationGameInstance* ROSInstance = UAgrarsenseStatics::GetROSGameInstance(GetWorld());
    if (ROSInstance && ROSInstance->IsROSConnected())
    {
        ROSTopic = NewObject<UTopic>(UTopic::StaticClass());
        if (ROSTopic)
        {
 
            FString TopicName = FString::Printf(TEXT("/agrarsense/out/vehicles/%s/height"), *GetActorID_Implementation());
            //UE_LOG(LogTemp, Warning, TEXT("TopicName is %s"), *TopicName);
 
            ROSTopic->Init(ROSInstance->ROSIntegrationCore, TopicName, "std_msgs/Float32");
            ROSTopic->Advertise();
        }
    }
}
 
float APIDDrone::GetYawRotationDifference(USkeletalMeshComponent* DroneMesh, UStaticMeshComponent* DesiredLocation)
{
    if (!DroneMesh || !DesiredLocation)
    {
        return 0.0f;
    }
 
    FVector ToTarget = DesiredLocation->GetComponentLocation() - DroneMesh->GetComponentLocation();
    FRotator LookRotation = ToTarget.Rotation();
 
    float CurrentYaw = DroneMesh->GetComponentRotation().Yaw;
    float DesiredYaw = LookRotation.Yaw;
 
    float DeltaYaw = FRotator::NormalizeAxis(DesiredYaw - CurrentYaw);
    return DeltaYaw;
}
 
void APIDDrone::DestroyTopic()
{
    if (ROSTopic)
    {
        ROSTopic->Unadvertise();
        ROSTopic->Unsubscribe();
        ROSTopic->MarkAsDisconnected();
        ROSTopic->ConditionalBeginDestroy();
        ROSTopic = nullptr;
    }
}