SimulatorJsonExporter.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 "SimulatorJsonExporter.h"
 
#include "Agrarsense/InstancedRendering/InstancedActor.h"
#include "Agrarsense/Utils/EnumUtilities.h"
#include "Agrarsense/Game/AgrarsensePaths.h"
#include "Agrarsense/Logging/SimulatorLog.h"
#include "Agrarsense/Walker/Walker.h"
 
#include "Agrarsense/Vehicle/Vehicle.h"
#include "Agrarsense/Vehicle/PIDDrone.h"
#include "Agrarsense/Vehicle/Forwarder.h"
#include "Agrarsense/Vehicle/Harvester.h"
 
#include "Agrarsense/Sensor/SensorsManagerComponent.h"
#include "Agrarsense/Sensor/Sensor.h"
#include "Agrarsense/Sensor/Radar/Radar.h"
#include "Agrarsense/Sensor/Lidar/Lidar.h"
#include "Agrarsense/Sensor/Camera/Camera.h"
#include "Agrarsense/Sensor/Camera/DepthCamera.h"
#include "Agrarsense/Sensor/Camera/DVSCamera.h"
#include "Agrarsense/Sensor/Camera/ThermalCamera.h"
#include "Agrarsense/Sensor/Camera/SemanticSegmentationCamera.h"
 
#include "GenericPlatform/GenericPlatformFile.h"
#include "Serialization/JsonSerializer.h"
#include "Serialization/JsonWriter.h"
#include "Misc/DefaultValueHelper.h"
#include "HAL/PlatformFileManager.h"
#include "Misc/FileHelper.h"
 
#include "Engine/GameViewportClient.h"
#include "Engine/Engine.h"
#include "Engine/World.h"
 
FString USimulatorJsonExporter::ExportVehicleAndSensorsToJSON(FString FileName, AVehicle* Vehicle, bool OverrideTransform, const FTransform& Transform)
{
    FString FilePath;
    if (!Vehicle)
    {
        return FilePath;
    }
 
    EVehicleTypes VehicleType = Vehicle->GetVehicleType();
    FString VehicleName;
 
    switch (VehicleType)
    {
    case EVehicleTypes::Forwarder:
        VehicleName = "Forwarder";
        break;
 
    case EVehicleTypes::Harvester:
        VehicleName = "Harvester";
        break;
 
    case EVehicleTypes::Drone:
        VehicleName = "Drone";
        break;
 
    case EVehicleTypes::NONE:
    default:
        break;
    }
 
    // If the vehicle name is empty, return
    if (VehicleName.IsEmpty())
    {
        return FilePath;
    }
 
    // Remove invalid characters from the filename, and replace them with undescores.
    // Will not remove spaces, maybe they should be?
    FString ValidatedFileName = FPaths::MakeValidFileName(FileName, TEXT('_'));
    FilePath = CreateUniqueFilePath(ValidatedFileName);
 
    TSharedPtr<FJsonObject> JsonObject = MakeShareable(new FJsonObject);
    TArray<TSharedPtr<FJsonValue>> ObjectsArray;
 
    TSharedPtr<FJsonObject> VehicleObject = MakeShareable(new FJsonObject);
    VehicleObject->SetStringField("type", "vehicle");
    VehicleObject->SetStringField("model", VehicleName);
    VehicleObject->SetBoolField("teleportSpectator", false);
    VehicleObject->SetBoolField("followObject", false);
    VehicleObject->SetBoolField("destroyOverlappingObjects", false);
    VehicleObject->SetStringField("id", Vehicle->GetActorID_Implementation());
 
    TSharedPtr<FJsonObject> VehicleSpawnPoint = MakeShareable(new FJsonObject);
 
    FVector SpawnLocation;
    FRotator SpawnRotation;
 
    if (OverrideTransform)
    {
        SpawnLocation = Transform.GetLocation();
        SpawnRotation = Transform.Rotator();
    }
    else
    {
        SpawnLocation = Vehicle->GetActorLocation();
        SpawnRotation = Vehicle->GetActorRotation();
    }
 
    // Export vehicle spawnpoint to json
    SetSpawnPoint(VehicleObject, SpawnLocation, SpawnRotation, Transform.GetScale3D());
 
    // If the vehicle is drone, export its parameters to json
    if (Vehicle->GetVehicleType() == EVehicleTypes::Drone)
    {
        SetDroneParameters(VehicleObject, Vehicle);
    }
 
    // Export all sensors attached to the vehicle
    TArray<TSharedPtr<FJsonValue>> SensorsArray;
 
    USensorsManagerComponent* SensorManager = Vehicle->GetSensorsManager();
    if (SensorManager)
    {
        // Parse different sensor types and add them to the SensorsArray
        SensorsArray.Append(ParseLidars(SensorManager->GetLidars()));
        SensorsArray.Append(ParseCameras(SensorManager->GetCameras()));
        SensorsArray.Append(ParseDepthCameras(SensorManager->GetDepthCameras()));
        SensorsArray.Append(ParseThermalCameras(SensorManager->GetThermalCameras()));
        SensorsArray.Append(ParseDVSCameras(SensorManager->GetDVSCameras()));
        SensorsArray.Append(ParseRadars(SensorManager->GetRadars()));
    }
 
    // Add the sensors array to the vehicle object if it's not empty
    if (SensorsArray.Num() > 0)
    {
        VehicleObject->SetArrayField("sensors", SensorsArray);
    }
 
    ObjectsArray.Add(MakeShareable(new FJsonValueObject(VehicleObject)));
 
    JsonObject->SetArrayField("objects", ObjectsArray);
 
    FString JsonString;
    TSharedRef<TJsonWriter<TCHAR>> JsonWriter = TJsonWriterFactory<>::Create(&JsonString);
    FJsonSerializer::Serialize(JsonObject.ToSharedRef(), JsonWriter);
 
    // Save the JSON string to the file
    bool Saved = FFileHelper::SaveStringToFile(JsonString, *FilePath);
    if (!Saved)
    {
        return FString();
    }
 
    // Publish and log info
    FString Msg = FString::Printf(TEXT("Exported vehicle JSON file to: %s"), *FilePath);
    SimulatorLog::Log(Msg);
 
    return FilePath;
}
 
FString USimulatorJsonExporter::ExportWeatherToJSON(const FString& FileName, const FWeatherParameters& WeatherParameters)
{
    // Remove invalid characters from the filename, and replace them with undescores.
    // Will not remove spaces, maybe they should be?
    FString ValidatedFileName = FPaths::MakeValidFileName(FileName, TEXT('_'));
 
    FString FilePath = CreateUniqueFilePath(ValidatedFileName);
 
    // Create the JSON object from FWeatherParameters struct
    TSharedPtr<FJsonObject> ParametersObject = MakeShareable(new FJsonObject);
    FJsonObjectConverter::UStructToJsonObject(FWeatherParameters::StaticStruct(), &WeatherParameters, ParametersObject.ToSharedRef());
 
    TSharedPtr<FJsonObject> RootObject = MakeShareable(new FJsonObject);
    TArray<TSharedPtr<FJsonValue>> ObjectsArray;
    TSharedPtr<FJsonObject> WeatherObject = MakeShareable(new FJsonObject);
    WeatherObject->SetStringField("type", "weather");
 
    WeatherObject->SetObjectField("parameters", ParametersObject);
 
    ObjectsArray.Add(MakeShareable(new FJsonValueObject(WeatherObject)));
 
    RootObject->SetArrayField("objects", ObjectsArray);
 
    FString JsonString;
    TSharedRef<TJsonWriter<>> JsonWriter = TJsonWriterFactory<>::Create(&JsonString);
    FJsonSerializer::Serialize(RootObject.ToSharedRef(), JsonWriter);
 
    bool Saved = FFileHelper::SaveStringToFile(JsonString, *FilePath);
    if (!Saved)
    {
        return FString();
    }
 
    // Publish and log info
    FString Msg = FString::Printf(TEXT("Exported Weather JSON file to: %s"), *FilePath);
    SimulatorLog::Log(Msg);
 
    return FilePath;
}
 
FString USimulatorJsonExporter::ExportInstancedActorsToJSON(const FString FileName, const TArray<AInstancedActor*>& Actors)
{
    // Remove invalid characters from the filename, and replace them with undescores.
    // Will not remove spaces, maybe they should be?
    FString ValidatedFileName = FPaths::MakeValidFileName(FileName, TEXT('_'));
 
    FString FilePath;
 
    if (Actors.IsEmpty())
    {
#if WITH_EDITOR
        UE_LOG(LogTemp, Warning, TEXT("ROSJsonExporter.cpp: Actors TArray is empty."));
#endif
        return FilePath;
    }
 
    FilePath = CreateUniqueFilePath(ValidatedFileName);
 
    TSharedPtr<FJsonObject> JsonObject = MakeShareable(new FJsonObject);
 
    TArray<TSharedPtr<FJsonValue>> ObjectsArray;
    for (AInstancedActor* Actor : Actors)
    {
        if (!Actor)
        {
            continue;
        }
 
        FString type = Actor->GetTypeIDString();
        FString model = Actor->GetModelIDString();
 
        // If these are not set, continue
        if (type == "none" || model == "none")
        {
            continue;
        }
 
        TSharedPtr<FJsonObject> ObjectEntry = MakeShareable(new FJsonObject);
        ObjectEntry->SetStringField("type", type);
        ObjectEntry->SetStringField("model", model);
 
        // Create a spawnPoint object
        TSharedPtr<FJsonObject> SpawnPoint = MakeShareable(new FJsonObject);
        FVector SpawnLocation = Actor->GetActorLocation();
        FRotator SpawnRotation = Actor->GetActorRotation();
        FVector Scale = Actor->GetActorScale();
        SetSpawnPoint(ObjectEntry, SpawnLocation, SpawnRotation, Scale);
        ObjectsArray.Add(MakeShareable(new FJsonValueObject(ObjectEntry)));
    }
 
    JsonObject->SetArrayField("objects", ObjectsArray);
 
    FString JsonString;
    TSharedRef<TJsonWriter<TCHAR>> JsonWriter = TJsonWriterFactory<>::Create(&JsonString);
    FJsonSerializer::Serialize(JsonObject.ToSharedRef(), JsonWriter);
 
    bool Saved = FFileHelper::SaveStringToFile(JsonString, *FilePath);
    if (!Saved)
    {
        return FString();
    }
 
    // Publish and log info
    FString Msg = FString::Printf(TEXT("Exported Actors JSON file to: %s"), *FilePath);
    SimulatorLog::Log(Msg);
 
    return FilePath;
}
 
FString USimulatorJsonExporter::ExportWalkerToJSON(const FString& FileName, AWalker* Walker)
{
    FString FilePath;
    if (!Walker)
    {
        return FilePath;
    }
 
    // Remove invalid characters from the filename, and replace them with undescores.
    // Will not remove spaces, maybe they should be?
    FString ValidatedFileName = FPaths::MakeValidFileName(FileName, TEXT('_'));
 
    FilePath = CreateUniqueFilePath(ValidatedFileName);
 
    // Root Json object
    TSharedPtr<FJsonObject> RootJsonObject = MakeShareable(new FJsonObject);
 
    // Json structure "Objects" array
    TArray<TSharedPtr<FJsonValue>> ObjectsArray;
    TSharedPtr<FJsonObject> ObjectEntry = MakeShareable(new FJsonObject);
 
    // Get Walker parameters
    FWalkerParameters WalkerParameters = Walker->GetWalkerParameters();
 
    // Set type and model
    ObjectEntry->SetStringField("type", "Walker");
    ObjectEntry->SetStringField("model", UEnumUtilities::ConvertWalkerTypeToString(WalkerParameters.WalkerType));
    ObjectEntry->SetStringField("id", Walker->GetActorID_Implementation());
 
    // Create and set the JSON object from FWeatherParameters struct
    TSharedPtr<FJsonObject> ParametersObject = MakeShareable(new FJsonObject);
    FJsonObjectConverter::UStructToJsonObject(FWalkerParameters::StaticStruct(), &WalkerParameters, ParametersObject.ToSharedRef());
 
    // Remove "Points" field
    ParametersObject->RemoveField(TEXT("points"));
 
    if (WalkerParameters.Points.Num() >= 2)
    {
        // If we have at least 2 Points, re-create "points" field manually 
        // so it's compatible with ROSJsonParser Transform parsing logic.
        TSharedPtr<FJsonObject> PointsObject = MakeShareable(new FJsonObject);
        for (int32 i = 0; i < WalkerParameters.Points.Num(); ++i)
        {
            FString PointName = FString::Printf(TEXT("point%d"), i);
 
            FTransform Transform = WalkerParameters.Points[i];
            FVector Location = Transform.GetLocation();
            FRotator Rotation = Transform.Rotator();
 
            // Adding location and rotation to the point data
            TSharedPtr<FJsonObject> PointData = MakeShareable(new FJsonObject);
            PointData->SetNumberField(TEXT("x"), Location.X);
            PointData->SetNumberField(TEXT("y"), Location.Y);
            PointData->SetNumberField(TEXT("z"), Location.Z);
            PointData->SetNumberField(TEXT("roll"), Rotation.Roll);
            PointData->SetNumberField(TEXT("pitch"), Rotation.Pitch);
            PointData->SetNumberField(TEXT("yaw"), Rotation.Yaw);
 
            // Adding point data to the PointsObject
            PointsObject->SetObjectField(PointName, PointData);
        }
 
        // Add created PointsObject to ParametersObject "points" field
        ParametersObject->SetObjectField(TEXT("points"), PointsObject);
    }
    else
    {
        // else create a "spawnPoint" since we don't have enough Points in the WalkerParameters.
        TSharedPtr<FJsonObject> SpawnPoint = MakeShareable(new FJsonObject);
 
        FVector SpawnLocation;
        FRotator SpawnRotation;
        if (WalkerParameters.Points.IsEmpty())
        {
            // If for whatever reason Points is empty, 
            // use Walker current location and rotation.
            SpawnLocation = Walker->GetActorLocation();
            SpawnRotation = Walker->GetActorRotation();
        }
        else
        {
            // Else use first point which should be the spawn point of the Walker
            FTransform Transform = WalkerParameters.Points[0];
            SpawnLocation = Transform.GetLocation();
            SpawnRotation = Transform.Rotator();
        }
 
        SetSpawnPoint(ParametersObject, SpawnLocation, SpawnRotation, Walker->GetActorScale());
    }
 
    // Set the parameters object in the entry
    ObjectEntry->SetObjectField("parameters", ParametersObject);
 
    // Add the entry to the ObjectsArray
    ObjectsArray.Add(MakeShareable(new FJsonValueObject(ObjectEntry)));
 
    // Set the ObjectsArray in the root JSON object
    RootJsonObject->SetArrayField("objects", ObjectsArray);
 
    // Serialize the root JSON object to a string
    FString JsonString;
    TSharedRef<TJsonWriter<TCHAR>> JsonWriter = TJsonWriterFactory<>::Create(&JsonString);
    FJsonSerializer::Serialize(RootJsonObject.ToSharedRef(), JsonWriter);
 
    // Save the JSON string to a file
    bool Saved = FFileHelper::SaveStringToFile(JsonString, *FilePath);
    if (!Saved)
    {
        // If saving fails, return empty string
        return FString();
    }
 
    // Publish and log info
    FString Msg = FString::Printf(TEXT("Exported Actors JSON file to: %s"), *FilePath);
    SimulatorLog::Log(Msg);
 
    return FilePath;
}
 
FString USimulatorJsonExporter::ExportSensorToJSON(const FString& FileName, ASensor* Sensor)
{
    FString FilePath;
    if (!Sensor)
    {
        return FilePath;
    }
 
    // Remove invalid characters from the filename, and replace them with undescores.
    // Will not remove spaces, maybe they should be?
    FString ValidatedFileName = FPaths::MakeValidFileName(FileName, TEXT('_'));
 
    FilePath = CreateUniqueFilePath(ValidatedFileName);
 
    ESensorTypes SensorType = Sensor->GetSensorType();
    FString SensorTypeString = UEnumUtilities::ConvertSensorTypeToString(SensorType);
 
    TSharedPtr<FJsonObject> ParametersObject = MakeShareable(new FJsonObject);
 
    bool Success = false;
 
    switch (SensorType)
    {
    case ESensorTypes::Lidar:
    {
        ALidar* LidarPtr = Cast<ALidar>(Sensor);
        if (LidarPtr)
        {
            FLidarParameters LidarParameters = LidarPtr->GetLidarParameters();
            FJsonObjectConverter::UStructToJsonObject(FLidarParameters::StaticStruct(), &LidarParameters, ParametersObject.ToSharedRef());
            Success = true;
        }
    }
    break;
 
    // RGB and Semantic segmentation cameras share same parameters
    case ESensorTypes::RGBCamera:
    case ESensorTypes::SemanticSegmentationCamera:
    {
        ACamera* CameraPtr = Cast<ACamera>(Sensor);
        if (CameraPtr)
        {
            ParametersObject = CameraParametersToJSONObject(CameraPtr->GetCameraParameters());
            Success = true;
        }
    }
    break;
 
    case ESensorTypes::ThermalCamera:
    {
        AThermalCamera* ThermalCameraPtr = Cast<AThermalCamera>(Sensor);
        if (ThermalCameraPtr)
        {
            FThermalCameraParameters ThermalCameraParameters = ThermalCameraPtr->GetThermalCameraParameters();
            FJsonObjectConverter::UStructToJsonObject(FThermalCameraParameters::StaticStruct(), &ThermalCameraParameters, ParametersObject.ToSharedRef());
            Success = true;
        }
    }
    break;
 
    case ESensorTypes::DepthCamera:
    {
        ACamera* CameraPtr = Cast<ACamera>(Sensor);
        if (CameraPtr)
        {
            ADepthCamera* DepthCameraPtr = Cast<ADepthCamera>(Sensor);
            if (DepthCameraPtr)
            {
                FDepthCameraParameters DepthCameraParameters = DepthCameraPtr->GetDepthCameraParameters();
                FJsonObjectConverter::UStructToJsonObject(FDepthCameraParameters::StaticStruct(), &DepthCameraParameters, ParametersObject.ToSharedRef());
            }
            Success = true;
        }
    }
    break;
 
    case ESensorTypes::DVSCamera:
    {
        ADVSCamera* DVSPtr = Cast<ADVSCamera>(Sensor);
        if (DVSPtr)
        {
            FDVSCameraParameters CameraParameters = DVSPtr->GetDVSCameraParameters();
            FJsonObjectConverter::UStructToJsonObject(FDVSCameraParameters::StaticStruct(), &CameraParameters, ParametersObject.ToSharedRef());
            Success = true;
        }
    }
    break;
 
    case ESensorTypes::Radar:
    {
        ARadar* RadaPtr = Cast<ARadar>(Sensor);
        if (RadaPtr)
        {
            FRadarParameters RadarParameters = RadaPtr->GetRadarParameters();
            FJsonObjectConverter::UStructToJsonObject(FRadarParameters::StaticStruct(), &RadarParameters, ParametersObject.ToSharedRef());
            Success = true;
        }
    }
    break;
 
    // Don't export these sensors since these are automatically created for Vehicles
    case ESensorTypes::Collision:
    case ESensorTypes::Transform:
    case ESensorTypes::Overlap:
#if WITH_EDITOR
        UE_LOG(LogTemp, Warning, TEXT("ROSJsonExporter.cpp: Exporting Collision/Transform or Overlap sensor is not supported."));
#endif
        break;
 
    default:
        break;
    }
 
    if (!Success)
    {
        return FString();
    }
 
    // Root Json object
    TSharedPtr<FJsonObject> RootJsonObject = MakeShareable(new FJsonObject);
 
    // Json structure "Objects" array
    TArray<TSharedPtr<FJsonValue>> ObjectsArray;
    TSharedPtr<FJsonObject> ObjectEntry = MakeShareable(new FJsonObject);
 
    // Set type and model
    ObjectEntry->SetStringField("type", "Sensor");
    ObjectEntry->SetStringField("model", SensorTypeString);
    ObjectEntry->SetStringField("id", Sensor->GetActorID_Implementation());
 
    FVector SpawnLocation = Sensor->GetActorLocation();
    FRotator SpawnRotation = Sensor->GetActorRotation();
    SetSpawnPoint(ObjectEntry, SpawnLocation, SpawnRotation, Sensor->GetActorScale());
 
    // Set the parameters object to ObjectEntry
    ObjectEntry->SetObjectField("parameters", ParametersObject);
 
    // Add the entry to ObjectsArray
    ObjectsArray.Add(MakeShareable(new FJsonValueObject(ObjectEntry)));
 
    // Set the ObjectsArray in the root JSON object
    RootJsonObject->SetArrayField("objects", ObjectsArray);
 
    // Serialize the root JSON object to a string
    FString JsonString;
    TSharedRef<TJsonWriter<TCHAR>> JsonWriter = TJsonWriterFactory<>::Create(&JsonString);
    FJsonSerializer::Serialize(RootJsonObject.ToSharedRef(), JsonWriter);
 
    // Save the JSON string to a file
    bool Saved = FFileHelper::SaveStringToFile(JsonString, *FilePath);
    if (!Saved)
    {
        return FString();
    }
 
    // Publish and log info
    FString Msg = FString::Printf(TEXT("Exported Actors JSON file to: %s"), *FilePath);
    SimulatorLog::Log(Msg);
 
    return FilePath;
}
 
TArray<TSharedPtr<FJsonValue>> USimulatorJsonExporter::ParseLidars(const TArray<ALidar*>& Lidars)
{
    TArray<TSharedPtr<FJsonValue>> LidarArray;
 
    for (ALidar* Lidar : Lidars)
    {
        if (!Lidar)
        {
            continue;
        }
 
        FTransform Transform = Lidar->GetRootComponent()->GetRelativeTransform();
        TSharedPtr<FJsonObject> SensorObject = CreateSensorJSONObject(Cast<ASensor>(Lidar), "Lidar");
 
        TSharedPtr<FJsonObject> ParametersObject = MakeShareable(new FJsonObject);
        FLidarParameters Params = Lidar->GetLidarParameters();
 
        FJsonObjectConverter::UStructToJsonObject(FLidarParameters::StaticStruct(), &Params, ParametersObject.ToSharedRef());
 
        SensorObject->SetObjectField("parameters", ParametersObject);
 
        LidarArray.Add(MakeShareable(new FJsonValueObject(SensorObject)));
    }
 
    return LidarArray;
}
 
TArray<TSharedPtr<FJsonValue>> USimulatorJsonExporter::ParseCameras(const TArray<ACamera*>& Cameras)
{
    TArray<TSharedPtr<FJsonValue>> CameraArray;
 
    for (ACamera* Camera : Cameras)
    {
        if (!Camera)
        {
            continue;
        }
 
        FTransform Transform = Camera->GetRootComponent()->GetRelativeTransform();
        TSharedPtr<FJsonObject> SensorObject = CreateSensorJSONObject(Cast<ASensor>(Camera), "RGBCamera");
 
        FCameraBaseParameters Params = Camera->GetCameraParameters();
 
        TSharedPtr<FJsonObject> ParametersObject = CameraParametersToJSONObject(Params);
 
        SensorObject->SetObjectField("parameters", ParametersObject);
 
        CameraArray.Add(MakeShareable(new FJsonValueObject(SensorObject)));
    }
 
    return CameraArray;
}
 
TArray<TSharedPtr<FJsonValue>> USimulatorJsonExporter::ParseDepthCameras(const TArray<ADepthCamera*>& DepthCameras)
{
    TArray<TSharedPtr<FJsonValue>> CameraArray;
 
    for (ADepthCamera* Camera : DepthCameras)
    {
        if (!Camera)
        {
            continue;
        }
 
        FTransform Transform = Camera->GetRootComponent()->GetRelativeTransform();
        TSharedPtr<FJsonObject> SensorObject = CreateSensorJSONObject(Cast<ASensor>(Camera), "DepthCamera");
 
        TSharedPtr<FJsonObject> DepthCameraParametersObject = MakeShareable(new FJsonObject);
 
        FDepthCameraParameters Params = Camera->GetDepthCameraParameters();
        FJsonObjectConverter::UStructToJsonObject(FDepthCameraParameters::StaticStruct(), &Params, DepthCameraParametersObject.ToSharedRef());
 
        SensorObject->SetObjectField("parameters", DepthCameraParametersObject);
 
        CameraArray.Add(MakeShareable(new FJsonValueObject(SensorObject)));
    }
 
    return CameraArray;
}
 
TArray<TSharedPtr<FJsonValue>> USimulatorJsonExporter::ParseThermalCameras(const TArray<AThermalCamera*>& ThermalCameras)
{
    TArray<TSharedPtr<FJsonValue>> CameraArray;
 
    for (AThermalCamera* Camera : ThermalCameras)
    {
        if (!Camera)
        {
            continue;
        }
 
        FTransform Transform = Camera->GetRootComponent()->GetRelativeTransform();
        TSharedPtr<FJsonObject> SensorObject = CreateSensorJSONObject(Cast<ASensor>(Camera), "ThermalCamera");
 
        TSharedPtr<FJsonObject> ThermalCameraParametersObject = MakeShareable(new FJsonObject);
 
        FThermalCameraParameters Params = Camera->GetThermalCameraParameters();
        FJsonObjectConverter::UStructToJsonObject(FThermalCameraParameters::StaticStruct(), &Params, ThermalCameraParametersObject.ToSharedRef());
 
        SensorObject->SetObjectField("parameters", ThermalCameraParametersObject);
 
        CameraArray.Add(MakeShareable(new FJsonValueObject(SensorObject)));
    }
 
    return CameraArray;
}
 
TArray<TSharedPtr<FJsonValue>> USimulatorJsonExporter::ParseDVSCameras(const TArray<ADVSCamera*>& DVSCameras)
{
    TArray<TSharedPtr<FJsonValue>> CameraArray;
 
    for (ADVSCamera* Camera : DVSCameras)
    {
        if (!Camera)
        {
            continue;
        }
 
        FTransform Transform = Camera->GetRootComponent()->GetRelativeTransform();
        TSharedPtr<FJsonObject> SensorObject = CreateSensorJSONObject(Cast<ASensor>(Camera), "DVSCamera");
 
        TSharedPtr<FJsonObject> DVSCameraParametersObject = MakeShareable(new FJsonObject);
 
        FDVSCameraParameters Params = Camera->GetDVSCameraParameters();
        FJsonObjectConverter::UStructToJsonObject(FDVSCameraParameters::StaticStruct(), &Params, DVSCameraParametersObject.ToSharedRef());
 
        SensorObject->SetObjectField("parameters", DVSCameraParametersObject);
 
        CameraArray.Add(MakeShareable(new FJsonValueObject(SensorObject)));
    }
 
    return CameraArray;
}
 
TArray<TSharedPtr<FJsonValue>> USimulatorJsonExporter::ParseRadars(const TArray<ARadar*>& Radars)
{
    TArray<TSharedPtr<FJsonValue>> RadarArray;
 
    for (ARadar* Radar : Radars)
    {
        if (!Radar)
        {
            continue;
        }
 
        FTransform Transform = Radar->GetRootComponent()->GetRelativeTransform();
        TSharedPtr<FJsonObject> SensorObject = CreateSensorJSONObject(Cast<ASensor>(Radar), "Radar");
 
        TSharedPtr<FJsonObject> RadarParametersObject = MakeShareable(new FJsonObject);
 
        FRadarParameters Params = Radar->GetRadarParameters();
        FJsonObjectConverter::UStructToJsonObject(FRadarParameters::StaticStruct(), &Params, RadarParametersObject.ToSharedRef());
 
        SensorObject->SetObjectField("parameters", RadarParametersObject);
 
        RadarArray.Add(MakeShareable(new FJsonValueObject(SensorObject)));
    }
 
    return RadarArray;
}
 
TSharedPtr<FJsonObject> USimulatorJsonExporter::CreateSensorJSONObject(const ASensor* SensorPtr, FString SensorName)
{
    TSharedPtr<FJsonObject> SensorObject = MakeShareable(new FJsonObject);
 
    if (!SensorPtr)
    {
#if WITH_EDITOR
        UE_LOG(LogTemp, Warning, TEXT("ROSJsonExporter.cpp: SensorPtr is nullptr!"));
#endif
        return SensorObject;
    }
 
    FJsonObject* SensorJsonObject = SensorObject.Get();
    SensorJsonObject->SetStringField("type", "sensor");
    SensorJsonObject->SetStringField("model", SensorName);
    SensorJsonObject->SetStringField("name", SensorPtr->GetSensorName());
    SensorJsonObject->SetStringField("id", SensorPtr->GetActorID_Implementation());
    SensorJsonObject->SetStringField("attachedToComponent", SensorPtr->AttachedToComponent);
    SensorJsonObject->SetStringField("attachedToBone", SensorPtr->AttachedToBone.ToString());
    
    FTransform Transform = SensorPtr->GetRootComponent()->GetRelativeTransform();
    FVector SensorLocation = Transform.GetLocation();
    FRotator SensorRotation = Transform.Rotator();
    SetSpawnPoint(SensorObject, SensorLocation, SensorRotation, Transform.GetScale3D());
 
    return SensorObject;
}
 
TSharedPtr<FJsonObject> USimulatorJsonExporter::CameraParametersToJSONObject(const FCameraBaseParameters& Parameters)
{
    TSharedPtr<FJsonObject> ParametersObject = MakeShareable(new FJsonObject);
    FJsonObjectConverter::UStructToJsonObject(FCameraBaseParameters::StaticStruct(), &Parameters, ParametersObject.ToSharedRef());
 
    return ParametersObject;
}
 
void USimulatorJsonExporter::SetSpawnPoint(TSharedPtr<FJsonObject>& ObjectEntry, const FVector& Location, const FRotator& Rotation, const FVector& Scale)
{
    TSharedPtr<FJsonObject> SpawnPoint = MakeShareable(new FJsonObject);
 
    // Set Location
    SpawnPoint->SetNumberField("x", Location.X);
    SpawnPoint->SetNumberField("y", Location.Y);
    SpawnPoint->SetNumberField("z", Location.Z);
 
    // Set Rotation
    SpawnPoint->SetNumberField("roll", Rotation.Roll);
    SpawnPoint->SetNumberField("pitch", Rotation.Pitch);
    SpawnPoint->SetNumberField("yaw", Rotation.Yaw);
 
    // Set Scale
    SpawnPoint->SetNumberField("scaleX", Scale.X);
    SpawnPoint->SetNumberField("scaleY", Scale.Y);
    SpawnPoint->SetNumberField("scaleZ", Scale.Z);
 
    ObjectEntry->SetObjectField("spawnPoint", SpawnPoint);
}
 
void USimulatorJsonExporter::SetDroneParameters(TSharedPtr<FJsonObject>& ObjectEntry, AVehicle* VehiclePtr)
{
    APIDDrone* DronePtr = Cast<APIDDrone>(VehiclePtr);
    if (!ObjectEntry.IsValid() || !DronePtr)
    {
        return;
    }
 
    TSharedPtr<FJsonObject> VehicleParams = MakeShareable(new FJsonObject);
 
    FDroneParameters DroneParams = DronePtr->GetDroneParameters();
 
    FString DroneActionString = UEnum::GetDisplayValueAsText(DroneParams.DroneAction).ToString();
    VehicleParams->SetStringField("droneAction", DroneActionString);
 
    FString DroneEndActionString = UEnum::GetDisplayValueAsText(DroneParams.DroneEndAction).ToString();
    VehicleParams->SetStringField("DroneEndAction", DroneEndActionString);
 
    if (!DroneParams.Points.IsEmpty())
    {
        // Create an array to hold the points
        TArray<TSharedPtr<FJsonValue>> PointsArray;
        PointsArray.Reserve(DroneParams.Points.Num());
 
        for (const FTransform& Transform : DroneParams.Points)
        {
            TSharedPtr<FJsonObject> PointObject = MakeShareable(new FJsonObject);
 
            // Serialize translation
            FVector Translation = Transform.GetLocation();
            PointObject->SetNumberField("X", Translation.X);
            PointObject->SetNumberField("Y", Translation.Y);
            PointObject->SetNumberField("Z", Translation.Z);
 
            // Serialize rotation (as a rotator or quaternion)
            FRotator Rotation = Transform.GetRotation().Rotator();
            PointObject->SetNumberField("Pitch", Rotation.Pitch);
            PointObject->SetNumberField("Yaw", Rotation.Yaw);
            PointObject->SetNumberField("Roll", Rotation.Roll);
 
            // Add the point object to the array
            PointsArray.Add(MakeShareable(new FJsonValueObject(PointObject)));
        }
 
        // Add the points array to the VehicleParams JSON object
        VehicleParams->SetArrayField("points", PointsArray);
    }
 
    // Add the VehicleParams object to the main VehicleObject
    ObjectEntry->SetObjectField("parameters", VehicleParams);
}
 
FString USimulatorJsonExporter::CreateUniqueFilePath(FString FileName)
{
    if (FileName.IsEmpty())
    {
        // If user didn't specify file name, give it a default name
        FileName = "ExportedJsonFile";
    }
 
    // Trim leading and trailing whitespace
    FileName = FileName.TrimStartAndEnd();
 
    // Check for invalid characters
    FString InvalidChars = TEXT("\\/:*?\"<>|");
    for (TCHAR InvalidChar : InvalidChars)
    {
        FileName.ReplaceCharInline(InvalidChar, TEXT('_'), ESearchCase::CaseSensitive);
    }
 
    FString DataPathFolder = UAgrarsensePaths::GetDataFolder();
    FString BaseFileName = FileName;
    FString FinalPath;
 
    int32 FileIndex = 0;
 
    // To avoid overriding existing file, 
    // find available filename by adding FILENAME_[NUM].json to the end
    do
    {
        FString IndexString = (FileIndex > 0) ? FString::Printf(TEXT("_%d"), FileIndex) : FString();
        FinalPath = DataPathFolder / FString::Printf(TEXT("ExportedJsonFiles/%s%s.json"), *BaseFileName, *IndexString);
        FileIndex++;
    } while (FPlatformFileManager::Get().GetPlatformFile().FileExists(*FinalPath));
 
 
    return FinalPath;
}