Vehicle.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 "Vehicle.h"
 
#include "Agrarsense/Sensor/Collision/CollisionSensor.h"
#include "Agrarsense/Sensor/Transform/TransformSensor.h"
#include "Agrarsense/Sensor/SensorsManagerComponent.h"
#include "Agrarsense/Sensor/Overlap/OverlapSensor.h"
#include "Agrarsense/Sensor/SensorFactory.h"
 
#include "Agrarsense/JSON/SimulatorJsonExporter.h"
#include "Agrarsense/Game/AgrarsenseStatics.h"
#include "Agrarsense/ROS/ROSHandler.h"
 
#include "Kismet/GameplayStatics.h"
#include "NiagaraFunctionLibrary.h"
#include "NiagaraComponent.h"
#include "NiagaraSystem.h"
 
AVehicle::AVehicle()
{
    PrimaryActorTick.bCanEverTick = false;
    SensorsManager = CreateDefaultSubobject<USensorsManagerComponent>(TEXT("SensorsManagerComp"));
    InteractableName = NSLOCTEXT("Agrarsense", "VehicleInteractableName", "Vehicle");
}
 
void AVehicle::BeginPlay()
{
    Super::BeginPlay();
 
    TogglePhysics(true);
 
    UROSHandler* ROSHandler = UAgrarsenseStatics::GetROSHandle(GetWorld());
    if (ROSHandler)
    {
        ROSHandler->OnROSStateChanged.AddUniqueDynamic(this, &AVehicle::ROSBridgeStateChanged);
    }
 
    bool IsDrone = GetVehicleType() == EVehicleTypes::Drone;
 
    // Spawn Transform and Collision sensors for each vehicle
    FTransform transform = GetTransform();
 
    // Create Collision sensor
    FString VehicleCollisionSensorID = ActorID + "/collision";
    CollisionSensor = USensorFactory::SpawnCollisionSensor(transform, this, VehicleCollisionSensorID, "collision", true, this);
 
    if (!IsDrone)
    {
        // Create Transform sensor for forwarder and harvester
        FTransformSensorParameters Params;
        Params.SaveTransformDataToDisk = true;
        Params.OwningActor = this;
 
        FString VehicleTransformSensorID = ActorID + "/transform";
        TransformSensor = USensorFactory::SpawnTransformSensor(transform, Params, VehicleTransformSensorID, "transform", true, this);
    }
 
 
    // Create Transform sensor for extra transform sensor positions 
    // (Front and Back for forwarder and harvester)
    for (UStaticMeshComponent* Comp : ExtraTransformSensorPositions)
    {
        if (Comp)
        {
            FString CompName = Comp->GetName();
 
            FTransformSensorParameters Params;
            Params.OwningActor = this;
            Params.SaveTransformDataToDisk = false;
            Params.UseOwningActorTransform = false;
            Params.UsePrimiteRelativeTransform = false;
            Params.PrimitiveComponent = Cast<UPrimitiveComponent>(Comp);
 
            const FString VehicleTransformSensorID = FString::Printf(TEXT("%s/%s/transform"), *ActorID, *CompName);
            //UE_LOG(LogTemp, Log, TEXT("Creating Transform Sensor: %s"), *VehicleTransformSensorID);
 
            ExtraTransformSensors.AddUnique(USensorFactory::SpawnTransformSensor(transform, Params, VehicleTransformSensorID, TEXT("transform"), true, this));
        }
    }
 
    float NiagaraComponentHeight = 500.0f;
    float OverlapBoundsSize = 50.0f;
 
    if (IsDrone)
    {
        NiagaraComponentHeight = 200.0f;
    }
 
    FVector RelativePosition = FVector(0.0f, 0.0f, 0.0f);
    if (GetVehicleType() == EVehicleTypes::Forwarder)
    {
        // TODO make better solution for this.
        RelativePosition = FVector(150.0f, -120.0f, 250.0f);
    }
 
    if (!IsDrone)
    {
        // Create overlap sensor for Forwarder and Harvester
        // Drone overlap sensor(s) are created in PIDDrone BeginPlay
        FOverlapSensorParameters Params;
        Params.OwningActor = this;
        Params.Size = FVector(OverlapBoundsSize, OverlapBoundsSize, OverlapBoundsSize);
        Params.RelativePosition = RelativePosition;
        FString OverlapSensorID = ActorID + "/overlap";
        OverlapSensor = USensorFactory::SpawnOverlapSensor(transform, Params, OverlapSensorID, "overlap", this);
    }
 
    // Load and setup rain/snow fall niagara system
    UNiagaraSystem* NiagaraSystem = LoadObject<UNiagaraSystem>(nullptr, TEXT("/Game/Agrarsense/Particles/WaterAndSnow/NS_Particles.NS_Particles"));
    if (NiagaraSystem)
    {
        NiagaraComponent = UNiagaraFunctionLibrary::SpawnSystemAttached(NiagaraSystem, this->GetRootComponent(),
            FName("NiagaraEmitterSocketName"),
            FVector(0.0, 0.0, NiagaraComponentHeight), FRotator::ZeroRotator, EAttachLocation::KeepRelativeOffset, true);
    }
    else
    {
        UE_LOG(LogTemp, Warning, TEXT("Vehicle.cpp: Couldn't find NS_Particles"));
    }
 
    ApplyVehicleParameters(VehicleParameters);
}
 
void AVehicle::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
 
    UROSHandler* ROSHandler = UAgrarsenseStatics::GetROSHandle(GetWorld());
    if (ROSHandler)
    {
        ROSHandler->OnROSStateChanged.RemoveDynamic(this, &AVehicle::ROSBridgeStateChanged);
    }
 
    if (TransformSensor)
    {
        TransformSensor->Destroy();
        TransformSensor = nullptr;
    }
 
    for (int32 i = 0; i < ExtraTransformSensors.Num(); i++)
    {
        if (ATransformSensor* SensorPtr = ExtraTransformSensors[i])
        {
            SensorPtr->Destroy();
        }
    }
    ExtraTransformSensors.Empty();
 
    if (CollisionSensor)
    {
        CollisionSensor->Destroy();
        CollisionSensor = nullptr;
    }
 
    if (OverlapSensor)
    {
        OverlapSensor->Destroy();
        OverlapSensor = nullptr;
    }
 
    // Destroy attached sensors when vehicle is destroyed
    if (IsValid(SensorsManager) && EndPlayReason == EEndPlayReason::Destroyed)
    {
        SensorsManager->DestroyAllSensors();
    }
 
    if (NiagaraComponent)
    {
        NiagaraComponent->UnregisterComponent();
        NiagaraComponent->DestroyComponent();
        NiagaraComponent = nullptr;
    }
 
    // Destroy all remaining Actors that are attached to this vehicle, if any
    TArray<AActor*> AttachedActors;
    GetAttachedActors(AttachedActors);
    for (AActor* Actor : AttachedActors)
    {
        if (Actor)
        {
            Actor->Destroy();
        }
    }
}
 
void AVehicle::ROSBridgeStateChanged(EROSState ROSState)
{
 
}
 
void AVehicle::SetVehicleParameters(FWheeledVehicleParameters NewParameters)
{
    VehicleParameters = NewParameters;
 
    if (OverlapSensor && GetVehicleType() != EVehicleTypes::Drone)
    {
        OverlapSensor->SetVisualizeOverlapArea(VehicleParameters.VisualizeOverlap);
 
        if (VehicleParameters.OverlapRadiusMeters > 0.0f)
        {
            const float BoundsSizeMEters = VehicleParameters.OverlapRadiusMeters;
            OverlapSensor->SetOverlapBounds(FVector(BoundsSizeMEters, BoundsSizeMEters, BoundsSizeMEters));
        }
    }
 
    // BlueprintImplementableEvent, this is handled in BP_Forwarder and BP_harvester.
    ApplyVehicleParameters(VehicleParameters);
}
 
void AVehicle::PossessedBy(AController* NewController)
{
    Super::PossessedBy(NewController);
 
    ResetCamera();
}
 
void AVehicle::TeleportVehicleTo_Implementation(FVector NewLocation, FRotator NewRotation)
{
    SetActorLocationAndRotation(NewLocation, NewRotation, false, nullptr, ETeleportType::TeleportPhysics);
}
 
void AVehicle::SetVisualizeVehicleOverlapArea(bool Visible)
{
    if (OverlapSensor)
    {
        OverlapSensor->SetVisualizeOverlapArea(Visible);
    }
}
 
FString AVehicle::ExportToJsonFile(const FString& FileName)
{
    bool OverrideTransform = IsVehicleInGarage();
 
    FTransform TransformToUse = GetActorTransform();
 
    if (OverrideTransform)
    {
        TransformToUse = MovedFromGarageTransform;
    }
 
    return USimulatorJsonExporter::ExportVehicleAndSensorsToJSON(FileName, this, OverrideTransform, TransformToUse);
}