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/AgrarsenseGameInstance.h"
#include "Agrarsense/Game/AgrarsenseStatics.h"
#include "Agrarsense/ROS/ROSHandler.h"
#include "Agrarsense/Sensor/SensorFactory.h"
#include "Agrarsense/Sensor/Transform/TransformSensorParameters.h"
 
#include "ROSIntegration/Classes/RI/Topic.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();
 
    ROSMessage.Reset();
    ROSMessage = MakeShared<ROSMessages::std_msgs::Float32>();
 
    CreateTopic();
 
    DroneSkeletalMesh = Cast<USkeletalMeshComponent>(GetComponentByClass(USkeletalMeshComponent::StaticClass()));
 
    PositionMesh = Cast<UStaticMeshComponent>(GetComponentByClass(UStaticMeshComponent::StaticClass()));
 
    StartingPosition = GetActorLocation();
 
 
    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(GetActorTransform(), Params, VehicleTransformSensorID, "transform", true, this);
    }
}
 
void APIDDrone::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
 
    ROSMessage.Reset();
    DestroyTopic();
}
 
void APIDDrone::Tick(float DeltaTime)
{
    Super::Tick(DeltaTime);
 
    if (!IsVehicleInGarage())
    {
        AutoPilot(DeltaTime);
        UpdateGroundHeight();
    }
}
 
void APIDDrone::AutoPilot(const float DeltaTime)
{
    if (DroneParameters.DroneAction == EDroneAction::Stationary)
    {
        return;
    }
 
    SetFlightpath();
}
 
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
 
    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)
{
    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 = 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::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();
        }
    }
}
 
void APIDDrone::DestroyTopic()
{
    if (ROSTopic)
    {
        ROSTopic->Unadvertise();
        ROSTopic->Unsubscribe();
        ROSTopic->MarkAsDisconnected();
        ROSTopic->ConditionalBeginDestroy();
        ROSTopic = nullptr;
    }
}