PIDDrone.h

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>.
 
#pragma once
 
#include "CoreMinimal.h"
#include "Vehicle.h"
#include "DroneParameters.h"
 
#include "Components/SkeletalMeshComponent.h"
#include "Components/StaticMeshComponent.h"
 
#include "ROSIntegration/Public/std_msgs/Float32.h"
 
#include "PIDDrone.generated.h"
 
class UTopic;
class AOverlapSensor;
 
DECLARE_DYNAMIC_MULTICAST_DELEGATE_OneParam(FDroneFinished, FTransform, _LastWaypoint);
 
UCLASS()
class AGRARSENSE_API APIDDrone : public AVehicle
{
    GENERATED_BODY()
 
public:
 
    APIDDrone();
 
    virtual EVehicleTypes GetVehicleType() const override { return EVehicleTypes::Drone; }
 
    virtual FTransform GetInteractableWorldTransform_Implementation() const override
    {
        if (DroneSkeletalMesh)
        {
            return DroneSkeletalMesh->GetComponentTransform();
        }
        else
        {
            return GetActorTransform();
        }
    }
 
    UFUNCTION(BlueprintCallable, Category = "Drone")
    void SetDroneAction(EDroneAction mode)
    {
        DroneParameters.DroneAction = mode;
    }
 
    UPROPERTY(BlueprintAssignable, Category = "Drone")
    FDroneFinished OnDroneFinished;
 
    UFUNCTION(BlueprintCallable, Category = "Drone")
    EDroneAction GetDroneAction() const
    {
        return DroneParameters.DroneAction;
    }
 
    UFUNCTION(BlueprintCallable, Category = "Drone")
    FDroneParameters GetDroneParameters() const
    {
        return DroneParameters;
    }
 
    UFUNCTION(BlueprintCallable, Category = "Drone")
    void ChangeDroneParameters(const FDroneParameters& newParameters);
 
    UFUNCTION(BlueprintCallable, Category = "Drone")
    void AddWayPoint(FVector Waypoint)
    {
        FTransform transform;
        transform.SetLocation(Waypoint);
 
        DroneParameters.Points.Add(transform);
    }
 
    UFUNCTION(BlueprintCallable, Category = "Drone")
    float GetDroneDistanceToNextPoint()
    {
        // On manual flight, return a fixed distance to not affect autofly max speed
        if (DroneParameters.DroneAction == EDroneAction::Manual)
        {
            return 5000;
        }
 
        return distanceToNextPoint;
    }
 
    UFUNCTION(BlueprintCallable, Category = "Drone")
    float GetDroneSpeed() const
    {
        if (!DroneSkeletalMesh)
        {
            return 0;
        }
 
        return FMath::Max(FMath::Abs(DroneSkeletalMesh->GetComponentVelocity().X), FMath::Abs(DroneSkeletalMesh->GetComponentVelocity().Y));
    }
 
    UFUNCTION(BlueprintCallable, Category = "Drone")
    void ClearWaypoints()
    {
        DroneParameters.Points.Empty();
    }
 
    UFUNCTION(BlueprintCallable, Category = "Drone")
    void SetFlightpath();
 
    UFUNCTION(BlueprintCallable, Category = "Drone")
    void SetManualControl(bool value)
    {
        manualControl = value;
    }
 
    void MoveDroneToPosition(const FTransform Transform);
 
    UFUNCTION(BlueprintCallable, Category = "Drone")
    TArray<FTransform> GenerateRoamingPoints(float radius, int32 roamingPoints);
 
    UFUNCTION(BlueprintCallable, Category = "Drone")
    float GetYawRotationDifference(USkeletalMeshComponent* DroneSkeletalMesh, UStaticMeshComponent* DesiredLocation);
 
    virtual void SetCurrentTransformToMovedFromGarageTransform() override
    {
        if (DroneSkeletalMesh)
        {
            MovedFromGarageTransform = DroneSkeletalMesh->GetComponentTransform();
            UE_LOG(LogTemp, Warning, TEXT("Moved from garage transform %s"), *MovedFromGarageTransform.ToString());
 
        }
        else
        {
            MovedFromGarageTransform = GetActorTransform();
            UE_LOG(LogTemp, Warning, TEXT("ACTOR Moved from garage transform WHAT IS THIS: %s"), *MovedFromGarageTransform.ToString());
 
        }
    }
 
    void SetShowForwardArrow(bool Show)
    {
        ShowForwardArrow = Show;
    }
 
    void SetDrawDebugPoints(bool show)
    {
        drawDebugPoints = show;
    }
 
    UFUNCTION(BlueprintCallable, Category = "Drone")
    bool GetShowForwardArrow() const
    {
        return ShowForwardArrow;
    }
 
    UFUNCTION(BlueprintCallable, Category = "Drone")
    USkeletalMeshComponent* GetDroneSkeletalMesh() const
    {
        return DroneSkeletalMesh;
    }
 
protected:
 
    void ROSBridgeStateChanged(EROSState ROSState) override;
 
    UPROPERTY()
    UTopic* ROSTopic = nullptr;
 
    TSharedPtr<ROSMessages::std_msgs::Float32> ROSMessage;
 
private:
 
    virtual void BeginPlay() override;
 
    virtual void Tick(float DeltaTime) override;
 
    virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override;
 
    void CreateTopic();
 
    void DestroyTopic();
 
    void HandleDestroy();
 
    void AutoPilot(const float DeltaTime);
 
    void DrawDebugPoints();
 
    UFUNCTION(BlueprintCallable)
    void AssignRoamingPoints(const TArray<FTransform> Points);
 
    UFUNCTION(BlueprintCallable)
    void SetDroneRotation(USkeletalMeshComponent* target, FRotator rotator);
 
    void UpdateGroundHeight();
 
    UFUNCTION(BlueprintCallable)
    bool IsRoaming() const
    {
        return DroneParameters.DroneAction == EDroneAction::Roaming;
    }
 
    UFUNCTION(BlueprintCallable)
    void SetDesiredLocationMesh(UStaticMeshComponent* meshcomponent)
    {
        PositionMesh = meshcomponent;
    }
 
    UFUNCTION(BlueprintCallable)
    FVector GetCurrentWaypointTarget();
 
    UFUNCTION(BlueprintCallable)
    FTransform GetCurrentWaypointTarget_Transform();
 
    FDroneParameters DroneParameters;
 
    UPROPERTY()
    EFlightMode flightmode = EFlightMode::Roaming;
 
    UPROPERTY()
    UWorld* World = nullptr;
 
    FVector StartingPosition;
 
    UPROPERTY()
    USkeletalMeshComponent* DroneSkeletalMesh = nullptr;
 
    UPROPERTY()
    UStaticMeshComponent* PositionMesh = nullptr;
 
    UPROPERTY()
    AOverlapSensor* InnerOverlapSensor = nullptr;
 
    /* For testing purposes. Hardcoded coordinates */
    TArray<FVector> WayPoints = {
        {3790, 8050, 5000},
        {11779.536955,6566.541435,5000}
    };
 
    bool waypointReached = false;
 
    bool manualControl = false;
 
    bool ShowForwardArrow = false;
 
    bool drawDebugPoints = false;
 
    int32 passedWaypoints = 0;
 
    float DroneHeightFromGround = 0.0f;
 
    float distanceToNextPoint = 0.0f;
 
    TArray<FTransform> dronePath;
 
    FTimerHandle DestroyHandle;
};