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 "Agrarsense/Vehicle/PIDDrone.h"
#include "Agrarsense/Game/AgrarsenseStatics.h"
#include "Agrarsense/ROS/ROSHandler.h"
 
#include "Components/SkeletalMeshComponent.h"
#include "Components/StaticMeshComponent.h"
#include "Camera/CameraComponent.h"
#include "Field/FieldSystemNodes.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::BeginPlay()
{
    Super::BeginPlay();
 
    World = GetWorld();
 
    mesh = Cast<USkeletalMeshComponent>(GetComponentByClass(USkeletalMeshComponent::StaticClass()));
    desiredPositionMesh = Cast<UStaticMeshComponent>(GetComponentByClass(UStaticMeshComponent::StaticClass()));
 
    StartingPosition = GetActorLocation();
}
 
void APIDDrone::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
}
 
void APIDDrone::Tick(float DeltaTime)
{
    Super::Tick(DeltaTime);
 
    if (!IsVehicleInGarage())
    {
        AutoPilot(DeltaTime);
    }
}
 
void APIDDrone::FlySetFlightpath()
{
    // 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 = mesh->GetRelativeTransform().GetLocation();
 
    if (desiredPositionMesh)
    {
        // Doesn't work
        desiredPositionMesh->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
 
    if (FVector2f::Distance(FVector2f(waypoint.X, waypoint.Y), FVector2f(currentlocation.X, currentlocation.Y)) < 1000)
    {
        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;
                    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;
                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()
{
    // UE_LOG(LogTemp, Warning, TEXT(" % s"), *DroneParameters.Points[passedWaypoints].GetLocation().ToString());
    if (DroneParameters.Points.Num() <= passedWaypoints)
    {
        return FVector(0, 0, 0);
    }
 
    return DroneParameters.Points[passedWaypoints].GetLocation();
}
 
void APIDDrone::AssignRoamingPoints(const TArray<FTransform> Points)
{
    // Empty current waypoints if any
    ClearWaypoints();
 
    for (const FTransform& point : Points)
    {
        // Assing new waypoints to the array
        WayPoints.Add(point.GetLocation());
    }
}
 
void APIDDrone::SetDroneRotation(USkeletalMeshComponent* target, FRotator rotator)
{
    if (!target)
    {
        return;
    }
 
    FRotator rotation = FRotator(
        FMath::Clamp(rotator.Pitch, -2, 2),
        FMath::Clamp(rotator.Yaw, -2, 2),
        FMath::Clamp(rotator.Roll, -2, 2));
 
    FRotator currentRotation = target->GetRelativeRotation();
 
    FRotator rotationDifference = currentRotation - rotator;
 
    if (FMath::Abs(rotator.Pitch) > 0 && FMath::Abs(currentRotation.Pitch) < FMath::Abs(rotationDifference.Pitch))
    {
        target->AddRelativeRotation(FRotator(rotation.Pitch, 0, 0), false, nullptr, ETeleportType::TeleportPhysics);
    }
    if (FMath::Abs(rotator.Yaw) > 0 && FMath::Abs(currentRotation.Yaw) < FMath::Abs(rotationDifference.Yaw))
    {
        target->AddRelativeRotation(FRotator(0, rotation.Yaw, 0), false, nullptr, ETeleportType::TeleportPhysics);
    }
    if (FMath::Abs(rotator.Roll) > 0 && FMath::Abs(currentRotation.Roll) < FMath::Abs(rotationDifference.Roll))
    {
        target->AddRelativeRotation(FRotator(0, 0, rotation.Roll), false, nullptr, ETeleportType::TeleportPhysics);
    }
 
    if (rotator.Pitch == 0 || rotator.Yaw == 0 || rotator.Roll == 0)
    {
        target->AddRelativeRotation(FRotator(-FMath::Clamp(rotationDifference.Pitch, -2, 2), -FMath::Clamp(rotationDifference.Yaw, -2, 2), -FMath::Clamp(rotationDifference.Roll, -2, 2)), false, nullptr, ETeleportType::TeleportPhysics);
    }
}
 
TArray<FTransform> APIDDrone::GenerateRoamingPoints(float radius, int32 roamingPoints)
{
    TArray<FTransform> generatedRoamingPoints;
    generatedRoamingPoints.Reserve(roamingPoints);
 
    FVector currentPosition = this->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::AutoPilot(float DeltaTime)
{
    if (DroneParameters.DroneAction == EDroneAction::Stationary)
    {
        return;
    }
    
    FlySetFlightpath();
}