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;
 
 
    FTransform transform = GetTransform();
 
    if (!IsDrone)
    {
 
        // Create Collision sensor
        FString VehicleCollisionSensorID = ActorID + "/collision";
        FSensorSpawnParameters CollisionSensorSpawnParams;
        CollisionSensorSpawnParams.Transform = transform;
        CollisionSensorSpawnParams.SensorIdentifier = VehicleCollisionSensorID;
        CollisionSensorSpawnParams.SensorName = "collision";
        CollisionSensorSpawnParams.SimulateSensor = true;
        CollisionSensorSpawnParams.Parent = this;
 
        FCollisionSensorParameters FCollisionSensorSpawnParams;
        FCollisionSensorSpawnParams.OwningActor = this;
        FCollisionSensorSpawnParams.UseActorCollision = true;
 
        CollisionSensor = USensorFactory::SpawnCollisionSensor(CollisionSensorSpawnParams, FCollisionSensorSpawnParams);
 
        // Create Transform sensor for forwarder and harvester
        FString VehicleTransformSensorID = ActorID + "/transform";
        FSensorSpawnParameters TransformSensorSpawnParams;
        TransformSensorSpawnParams.Transform = transform;
        TransformSensorSpawnParams.SensorIdentifier = VehicleTransformSensorID;
        TransformSensorSpawnParams.SensorName = "transform";
        TransformSensorSpawnParams.SimulateSensor = true;
        TransformSensorSpawnParams.Parent = this;
 
        FTransformSensorParameters SensorParams;
        SensorParams.SaveTransformDataToDisk = true;
        SensorParams.OwningActor = this;
 
        TransformSensor = USensorFactory::SpawnTransformSensor(TransformSensorSpawnParams, SensorParams);
    }
 
 
    // If in the bluerprint side, we have added extra transform sensor positions, 
    // we will create Transform sensors for those positions as well (eg. forwarder/harvester front and back sensors).
    for (UStaticMeshComponent* Comp : ExtraTransformSensorPositions)
    {
        if (Comp)
        {
            FString CompName = Comp->GetName();
 
            const FString VehicleTransformSensorID = FString::Printf(TEXT("%s/%s/transform"), *ActorID, *CompName);
 
            FSensorSpawnParameters TransformSensorSpawnParams;
            TransformSensorSpawnParams.Transform = transform;
            TransformSensorSpawnParams.SensorIdentifier = VehicleTransformSensorID;
            TransformSensorSpawnParams.SensorName = "transform";
            TransformSensorSpawnParams.SimulateSensor = true;
            TransformSensorSpawnParams.Parent = this;
 
            FTransformSensorParameters Params;
            Params.OwningActor = this;
            Params.SaveTransformDataToDisk = false;
            Params.UseOwningActorTransform = false;
            Params.UsePrimiteRelativeTransform = false;
            Params.PrimitiveComponent = Cast<UPrimitiveComponent>(Comp);
 
            ExtraTransformSensors.AddUnique(USensorFactory::SpawnTransformSensor(TransformSensorSpawnParams, Params));
        }
    }
 
    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
 
        FString OverlapSensorID = ActorID + "/overlap";
 
        FSensorSpawnParameters OverlapSensorSpawnParams;
        OverlapSensorSpawnParams.Transform = transform;
        OverlapSensorSpawnParams.SensorIdentifier = OverlapSensorID;
        OverlapSensorSpawnParams.SensorName = "transform";
        OverlapSensorSpawnParams.SimulateSensor = true;
        OverlapSensorSpawnParams.Parent = this;
 
        FOverlapSensorParameters SensorParams;
        SensorParams.OwningActor = this;
        SensorParams.Size = FVector(OverlapBoundsSize, OverlapBoundsSize, OverlapBoundsSize);
        SensorParams.RelativePosition = RelativePosition;
        
        OverlapSensor = USensorFactory::SpawnOverlapSensor(OverlapSensorSpawnParams, SensorParams);
    }
 
    // 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);
}