ROSCommands.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 "ROSCommands.h"
 
#include "Agrarsense/AssetLibrary/AssetLibrary.h"
#include "Agrarsense/Logging/SimulatorLog.h"
#include "Agrarsense/ROS/ROSHandler.h"
#include "Agrarsense/JSON/SimulatorJsonParser.h"
#include "Agrarsense/JSON/SimulatorJsonExporter.h"
#include "Agrarsense/Logging/SimulatorLog.h"
 
#include "Agrarsense/Sensor/Overlap/OverlapSensor.h"
#include "Agrarsense/Sensor/Lidar/LidarManager.h"
#include "Agrarsense/Sensor/SensorUtilities.h"
#include "Agrarsense/Sensor/Sensor.h"
 
#include "Agrarsense/Walker/Walker.h"
#include "Agrarsense/Vehicle/Vehicle.h"
#include "Agrarsense/Vehicle/PIDDrone.h"
#include "Agrarsense/Weather/Weather.h"
#include "Agrarsense/Spectator/Spectator.h"
#include "Agrarsense/Settings/AgrarsenseSettings.h"
#include "Agrarsense/Game/AgrarsenseStatics.h"
#include "Agrarsense/InstancedRendering/InstancedRenderer.h"
#include "Agrarsense/ActorInterfaces/ActorInformation.h"
#include "Agrarsense/Volume/DeletionVolume.h"
 
#include "ROSIntegration/Classes/ROSIntegrationGameInstance.h"
#include "ROSIntegration/Public/std_msgs/String.h"
#include "ROSIntegration/Classes/RI/Topic.h"
 
#include "Engine/GameViewportClient.h"
#include "Engine/Engine.h"
#include "Engine/World.h"
#include "Misc/DateTime.h"
#include "Engine/World.h"
#include "Async/Async.h"
 
#include <functional>
 
TMap<FString, FCommand> UROSCommands::CommandHandlers;
 
void UROSCommands::Init()
{
    AddCommands();
 
    UROSHandler* ROSHandler = UAgrarsenseStatics::GetROSHandle(GetGameWorld());
    if (ROSHandler)
    {
        ROSHandler->OnROSStateChanged.AddUniqueDynamic(this, &UROSCommands::ROSBridgeStateChanged);
    }
 
    SetupROSCommandTopic();
}
 
void UROSCommands::Destroy()
{
    DestroyROSTopic();
}
 
void UROSCommands::TryExecuteCommand(const FString& Command)
{
    ParseIncomingMessage(Command);
}
 
UWorld* UROSCommands::GetGameWorld()
{
    UWorld* World = nullptr;
    if (GEngine && GEngine->GameViewport)
    {
        World = GEngine->GameViewport->GetWorld();
    }
    return World;
}
 
UAgrarsenseSettings* UROSCommands::GetSettings()
{
    return UAgrarsenseStatics::GetAgrarsenseSettings();
}
 
void UROSCommands::AddCommands()
{
    // To call from Terminal (note all in lower case)
    // rostopic pub /agrarsense/in/commands std_msgs/String "{COMMAND} {VARIABLE}" --once
    // rostopic pub /agrarsense/in/commands std_msgs/String "help" --once
 
    if (!CommandHandlers.IsEmpty())
    {
        return;
    }
 
    
    // General commands
    CommandHandlers.Add(TEXT("help"), FCommand(&UROSCommands::HandlePrintAvailableCommands, 0.1f, "Print all commands"));
    CommandHandlers.Add(TEXT("quit"), FCommand(&UROSCommands::HandleQuit, 1.0f, "Quits the Simulator"));
    CommandHandlers.Add(TEXT("loadmap"), FCommand(&UROSCommands::HandleChangeMap, 5.0f, TEXT("load map by name")));
    CommandHandlers.Add(TEXT("unrealcommand"), FCommand(&UROSCommands::HandleUnrealCommand, 0.05f, TEXT("Execute Unreal engine console command")));
 
    // Pause commands
    CommandHandlers.Add(TEXT("pause"), FCommand(&UROSCommands::HandlePauseSimulator, 0.01f, "Pauses the Simulator after current frame has finished"));
    CommandHandlers.Add(TEXT("unpause"), FCommand(&UROSCommands::HandleUnPauseSimulator, 0.01f, "Un pauses the Simulator immediately upon receiving the message"));
    CommandHandlers.Add(TEXT("nextframe"), FCommand(&UROSCommands::HandleAdvanceOneFrame, 0.01f, "Advance one frame"));
    CommandHandlers.Add(TEXT("advanceframes"), FCommand(&UROSCommands::HandleAdvanceFrames, 0.01f, "Advance given amount of frames"));
    CommandHandlers.Add(TEXT("advancetime"), FCommand(&UROSCommands::HandleAdvanceTime, 0.01f, "Advance given amount of time in seconds"));
 
    // Teleport commands
    CommandHandlers.Add(TEXT("teleportspectator"), FCommand(&UROSCommands::HandleTeleportSpectator, 0.05f, TEXT("x,y,z or x,y,z,yaw,pitch,roll")));
    CommandHandlers.Add(TEXT("teleportbyid"), FCommand(&UROSCommands::HandleTeleportActorByID, 0.05f, TEXT("ID of Actor, x,y,z or x,y,z,yaw,pitch,roll")));
 
    // Destroy commands
    CommandHandlers.Add(TEXT("destroyobjectbyid"), FCommand(&UROSCommands::HandleTryDestroyObjectByID, 0.05f, TEXT("Try to destroy object by id")));
    CommandHandlers.Add(TEXT("destroyallsensors"), FCommand(&UROSCommands::HandleDestroyAllSensors, 1.0f, TEXT("Destroy all sensors")));
    CommandHandlers.Add(TEXT("destroyallwalkers"), FCommand(&UROSCommands::HandleDestroyAllWalkers, 1.0f, TEXT("Destroy all sensors")));
    CommandHandlers.Add(TEXT("destroyallvehicles"), FCommand(&UROSCommands::HandleDestroyAllVehicles, 1.0f, TEXT("Destroy all sensors")));
 
    // Volume commands 
    // ex. "destroytrees 0,0,0 300,300,300" - destroys all tree actors within certain box area.
    // ex. "destroyassets 0,0,0 300,300,300" - destroys all Instanced actors within certain box area.
    CommandHandlers.Add(TEXT("destroytreesarea"), FCommand(&UROSCommands::HandleDestroyTreesArea, 0.05f, TEXT("x,y,z sizeX,sizeY,sizeZ")));
    CommandHandlers.Add(TEXT("destroyassetsarea"), FCommand(&UROSCommands::HandleDestroyActorsArea, 0.05f, TEXT("x,y,z sizeX,sizeY,sizeZ")));
 
    // Print commands
    CommandHandlers.Add(TEXT("getmaps"), FCommand(&UROSCommands::HandlePrintMaps, 0.5f, TEXT("Print all available maps")));
    CommandHandlers.Add(TEXT("getsensors"), FCommand(&UROSCommands::HandlePrintAllSensors, 0.1f, TEXT("Print all sensors information")));
    CommandHandlers.Add(TEXT("getvehicles"), FCommand(&UROSCommands::HandlePrintAllVehicles, 0.1f, TEXT("Print all vehicles information")));
    CommandHandlers.Add(TEXT("getwalkers"), FCommand(&UROSCommands::HandlePrintMaps, 0.1f, TEXT("print all walkers information")));
    CommandHandlers.Add(TEXT("getids"), FCommand(&UROSCommands::HandlePrintIds, 0.1f, TEXT("print all ids")));
 
    // Set commands
    CommandHandlers.Add(TEXT("setglobaltargetframeRate"), FCommand(&UROSCommands::HandleSetGlobalTargetFrameRate, 1.0f, TEXT("0 - 300")));
    CommandHandlers.Add(TEXT("setglobaltimedilation"), FCommand(&UROSCommands::HandleSetGlobalTimeDilation, 1.0f, TEXT("0.0f - 10.0f")));
    CommandHandlers.Add(TEXT("setqualityLevel"), FCommand(&UROSCommands::HandleSetQualityLevel, 2.0f, TEXT("low / ultra")));
    CommandHandlers.Add(TEXT("setworldrendering"), FCommand(&UROSCommands::HandleSetWorldRendering, 1.0f, TEXT("true / false")));
    CommandHandlers.Add(TEXT("setsavecombinedpointcloud"), FCommand(&UROSCommands::HandleSetSaveCombinedPointcloudToDisk, 0.05f, TEXT("true / false")));
    CommandHandlers.Add(TEXT("setwpodistance"), FCommand(&UROSCommands::HandleSetWPO, 0.5f, TEXT("Set WPO render distance in cm")));
    CommandHandlers.Add(TEXT("setnanitemaxpixelsperedge"), FCommand(&UROSCommands::HandleSetNaniteMaxPixelsPerEdge, 0.5f, TEXT("Set Nanite max pixels per edge")));
    
    // Sensor commands
    CommandHandlers.Add(TEXT("setallsensorsenabled"), FCommand(&UROSCommands::HandleSetAllSensorsEnabled, 1.0f, TEXT("true / false")));
    CommandHandlers.Add(TEXT("enableallsensors"), FCommand(&UROSCommands::HandleEnableAllSensors, 1.0f, TEXT("true / false")));
    CommandHandlers.Add(TEXT("disableallsensors"), FCommand(&UROSCommands::HandleDisableAllSensors, 1.0f, TEXT("true / false")));
    CommandHandlers.Add(TEXT("setsensorenabled"), FCommand(&UROSCommands::HandleSetSensorEnabled, 1.0f, TEXT("ID true/false")));
 
    // Spawn/change objects commands. They all call the same function. They are defined separately for clarity.
    CommandHandlers.Add(TEXT("spawnobjects"), FCommand(&UROSCommands::HandleSpawnObjects, 0.05f, TEXT("Spawn objects from JSON file")));
    CommandHandlers.Add(TEXT("changeweather"), FCommand(&UROSCommands::HandleSpawnObjects, 0.05f, TEXT("change weather from JSON file")));
    CommandHandlers.Add(TEXT("changecolors"), FCommand(&UROSCommands::HandleSpawnObjects, 0.05f, TEXT("change weather from JSON file")));
 
    // Exports
    CommandHandlers.Add(TEXT("exportall"), FCommand(&UROSCommands::HandleExportAll, 1.0f, TEXT("Export all spanwed things to JSON file")));
    CommandHandlers.Add(TEXT("exportweather"), FCommand(&UROSCommands::HandleExportWeather, 1.0f, TEXT("Export current weather to JSON file")));
    CommandHandlers.Add(TEXT("exportwalkers"), FCommand(&UROSCommands::HandleExportWalkers, 1.0f, TEXT("Export all Walkers to JSON file")));
    CommandHandlers.Add(TEXT("exportvehicles"), FCommand(&UROSCommands::HandleExportVehicles, 1.0f, TEXT("Export all Vehicles to JSON file")));
    CommandHandlers.Add(TEXT("exportsensors"), FCommand(&UROSCommands::HandleExportSensors, 1.0f, TEXT("Export all Sensors to JSON file")));
    CommandHandlers.Add(TEXT("exportprops"), FCommand(&UROSCommands::HandleExportProps, 1.0f, TEXT("Export all props to JSON file")));
    CommandHandlers.Add(TEXT("exportfoliage"), FCommand(&UROSCommands::HandleExportFoliage, 1.0f, TEXT("Export all Foliage to JSON file")));
    
    // Overlap sensor commands
    // changeoverlapbounds format: 'changeoverlapbounds sensor_id 500,500,500'
    // changeoverlapposition format: 'changeoverlapposition sensor_id 0,0,0'
    CommandHandlers.Add(TEXT("visoverlapbounds"), FCommand(&UROSCommands::HandleVisualizeOverlapSensorsBounds, 0.05f, TEXT("Visualize All Overlap sensors bounds")));
    CommandHandlers.Add(TEXT("changeoverlapbounds"), FCommand(&UROSCommands::HandleChangeOverlapSensorBounds, 0.05f, TEXT("Change Overlap sensor area bounds")));
    CommandHandlers.Add(TEXT("changeoverlapposition"), FCommand(&UROSCommands::HandleChangeOverlapSensorPosition, 0.05f, TEXT("Change Overlap sensor relative position")));
 
    // Walker commands
    CommandHandlers.Add(TEXT("movewalkerto"), FCommand(&UROSCommands::HandleMoveWalkerToAndStop, 0.05f, TEXT("ID (position or position,rotation)")));
    CommandHandlers.Add(TEXT("movewalkertoanddestroy"), FCommand(&UROSCommands::HandleMoveWalkerToAndDestroy, 0.05f, TEXT("ID (position or position,rotation)")));
    CommandHandlers.Add(TEXT("stopwalker"), FCommand(&UROSCommands::HandleStopWalker, 0.05f, TEXT("ID of the Walker")));
    CommandHandlers.Add(TEXT("resumewalker"), FCommand(&UROSCommands::HandleResumeWalker, 0.05f, TEXT("ID of the Walker")));
 
    // Drone commands
    CommandHandlers.Add(TEXT("movedroneto"), FCommand(&UROSCommands::HandleMoveDroneToAndStop, 0.05f, TEXT("ID (position)")));
}
 
void UROSCommands::ROSBridgeStateChanged(EROSState state)
{
    switch (state)
    {
    case EROSState::Connected:
        SetupROSCommandTopic();
        break;
 
    case EROSState::Disconnected:
        DestroyROSTopic();
        break;
    }
}
 
void UROSCommands::DestroyROSTopic()
{
    if (CommandTopic)
    {
        CommandTopic->Unadvertise();
        CommandTopic->Unsubscribe();
        CommandTopic->MarkAsDisconnected();
        CommandTopic->ConditionalBeginDestroy();
        CommandTopic = nullptr;
    }
}
 
void UROSCommands::SetupROSCommandTopic()
{
    UWorld* World = GetGameWorld();
    if (!World)
    {
        return;
    }
 
    RosInstance = UAgrarsenseStatics::GetROSGameInstance(World);
    if (!CommandTopic && RosInstance && RosInstance->IsROSConnected())
    {
        CommandTopic = NewObject<UTopic>(UTopic::StaticClass());
        CommandTopic->Init(RosInstance->ROSIntegrationCore, TEXT("/agrarsense/in/commands"), TEXT("std_msgs/String"));
        CommandTopic->Advertise();
 
        // Create a std::function callback object
        std::function<void(TSharedPtr<FROSBaseMsg>)> SubscribeCallback = [this](TSharedPtr<FROSBaseMsg> msg) -> void
            {
                auto Concrete = StaticCastSharedPtr<ROSMessages::std_msgs::String>(msg);
                if (Concrete.IsValid())
                {
                    FString message = *(Concrete->_Data);
                    AsyncTask(ENamedThreads::GameThread, [this, message]()
                        {
                            // Parse message on GameThread since many commands are NOT thread-safe
                            ParseIncomingMessage(message);
                        });
                }
                return;
            };
 
        // Subscribe to the topic
        CommandTopic->Subscribe(SubscribeCallback);
    }
}
 
void UROSCommands::ParseIncomingMessage(const FString Message)
{
    // Parse incoming string message into {COMMAND} {VARIABLE} format
    int32 SpaceIndex;
    FString Command;
    FString Variable;
 
    if (Message.FindChar(' ', SpaceIndex))
    {
        // Extract the command as the substring before the first space
        Command = Message.Left(SpaceIndex).ToLower();
 
        // Extract the rest of the message as the variable
        Variable = Message.Mid(SpaceIndex + 1).ToLower();
    }
    else
    {
        // If no space is found, consider the entire message as the command
        Command = Message.ToLower();
    }
 
#if WITH_EDITOR
    UE_LOG(LogTemp, Warning, TEXT("ROSCommands.Cpp: Incoming Command: %s with variable: %s"), *Command, *Variable);
#endif
 
    FCommand* CommandInfo = CommandHandlers.Find(Command);
    if (CommandInfo)
    {
        // Check if enough time has passed since the last execution.
        double CurrentTime = FPlatformTime::Seconds();
        double TimeSinceLastExecution = CurrentTime - CommandInfo->LastExecutionTime;
 
        if (TimeSinceLastExecution >= CommandInfo->CooldownTime)
        {
            // Call the handler function with the variable.
            (this->*(CommandInfo->Handler))(Variable);
 
            // Update the last execution time.
            CommandInfo->LastExecutionTime = CurrentTime;
        }
        else
        {
            float CoolDownTimeLeft = CommandInfo->CooldownTime - TimeSinceLastExecution;
            FString Msg = FString::Printf(TEXT("Command '%s' on cooldown. Retry in %.2f sec."), *Command, CoolDownTimeLeft);
            SimulatorLog::Log(Msg);
        }
    }
    else
    {
        FString Msg = FString::Printf(TEXT("Couldn't find command: '%s'"), *Command);
        SimulatorLog::Log(Msg);
    }
}
 
bool UROSCommands::TryParseBoolean(const FString& String, bool& OutBool)
{
    FString LoweredString = String.ToLower();
 
    if (LoweredString.Equals("true") || LoweredString.Equals("1"))
    {
        OutBool = true;
        return true;
    }
    else if (LoweredString.Equals("false") || LoweredString.Equals("0"))
    {
        OutBool = false;
        return true;
    }
 
    // Invalid boolean value
    return false;
}
 
void UROSCommands::HandlePrintAvailableCommands(const FString& Variable)
{
    if (!CommandTopic)
    {
        return;
    }
 
    FString message = "Available Simulator ROS commands {COMMAND} {VALUE} || ";
 
    for (const TPair<FString, FCommand>& CommandPair : CommandHandlers)
    {
        const FString& CommandString = CommandPair.Key;
        const FCommand& CommandInfo = CommandPair.Value;
        if (CommandInfo.DefaultValue.IsEmpty())
        {
            message += FString::Printf(TEXT("%s || "), *CommandString);
        }
        else
        {
            message += FString::Printf(TEXT("%s %s || "), *CommandString, *CommandInfo.DefaultValue);
        }
    }
 
    SimulatorLog::Log(message);
}
 
void UROSCommands::HandleQuit(const FString& Variable)
{
    UWorld* World = GetGameWorld();
    if (GEngine && World)
    {
        GEngine->Exec(World, TEXT("quit force"));
    }
}
 
void UROSCommands::HandlePauseSimulator(const FString& Variable)
{
    UAgrarsenseSettings* AgrarsenseSettings = GetSettings();
    if (AgrarsenseSettings)
    {
        AgrarsenseSettings->PauseSimulationEndOfThisFrame();
    }
}
 
void UROSCommands::HandleUnPauseSimulator(const FString& Variable)
{
    UAgrarsenseSettings* AgrarsenseSettings = GetSettings();
    if (AgrarsenseSettings)
    {
        AgrarsenseSettings->UnPauseSimulation();
    }
}
 
void UROSCommands::HandleAdvanceOneFrame(const FString& Variable)
{
    UAgrarsenseSettings* AgrarsenseSettings = GetSettings();
    if (AgrarsenseSettings)
    {
        AgrarsenseSettings->AdvanceFrameCount(1);
    }
}
 
void UROSCommands::HandleAdvanceFrames(const FString& Variable)
{
    int32 FrameCount = 0;
    if (!Variable.IsNumeric())
    {
        return;
    }
 
    FrameCount = FCString::Atoi(*Variable);
    if (FrameCount != 0) 
    {
        UAgrarsenseSettings* AgrarsenseSettings = GetSettings();
        if (AgrarsenseSettings)
        {
            AgrarsenseSettings->AdvanceFrameCount(FrameCount);
        }
    }
}
 
void UROSCommands::HandleAdvanceTime(const FString& Variable)
{
    float Time = 0.0f;
    if (!Variable.IsNumeric())
    {
        return;
    }
 
    Time = FCString::Atof(*Variable);
    if (Time != 0.0f)
    {
        UAgrarsenseSettings* AgrarsenseSettings = GetSettings();
        if (AgrarsenseSettings)
        {
            AgrarsenseSettings->AdvanceTime(Time);
        }
    }
}
 
void UROSCommands::HandleChangeMap(const FString& Variable)
{
    UAgrarsenseSettings* AgrarsenseSettings = GetSettings();
    if (AgrarsenseSettings)
    {
        AgrarsenseSettings->ChangeMapByName(Variable);
    }
}
 
void UROSCommands::HandleUnrealCommand(const FString& Variable)
{
    UWorld* World = GetGameWorld();
    if (GEngine && World)
    {
        GEngine->Exec(World, *Variable);
    }
}
 
void UROSCommands::HandleTeleportSpectator(const FString& Variable)
{
    // Split the comma-separated string into components
    TArray<FString> Components;
    Variable.ParseIntoArray(Components, TEXT(","), true);
 
    if (Components.Num() >= 3)
    {
        FVector Location;
        FQuat Rotation;
 
        // Convert the first three components to float values for location
        float X = FCString::Atof(*Components[0]);
        float Y = FCString::Atof(*Components[1]);
        float Z = FCString::Atof(*Components[2]);
 
        Location = FVector(X, Y, Z);
 
        if (Components.Num() >= 6)
        {
            // If there are six or more components, try to parse them as rotation values
            float Pitch = FCString::Atof(*Components[3]);
            float Yaw = FCString::Atof(*Components[4]);
            float Roll = FCString::Atof(*Components[5]);
 
            // Convert the Euler angles to a quaternion
            Rotation = FQuat(FRotator(Pitch, Yaw, Roll));
        }
 
        UWorld* World = GetGameWorld();
        if (World)
        {
            ASpectator* Spectator = UAgrarsenseStatics::GetSpectator(World);
            if (Spectator)
            {
                FTransform Transform;
                Transform.SetLocation(Location);
                Transform.SetRotation(Rotation);
                Spectator->TeleportSpectator(Transform);
            }
        }
    }
}
 
void UROSCommands::HandleTeleportActorByID(const FString& Variable)
{
    FString ID;
    FString TransformString;
 
    // Split the ID and rest of the message
    TArray<FString> Tokens;
    Variable.ParseIntoArray(Tokens, TEXT(" "), true);
    if (Tokens.Num() >= 2)
    {
        ID = Tokens[0];
        TransformString = Variable.RightChop(ID.Len() + 1);
    }
    else
    {
        // Not valid, return
        return;
    }
 
    // Check if this ID matches any Actor
    AActor* Actor = IActorInformation::GetActorByID(ID);
    if (!Actor)
    {
        // Not valid, return
        return;
    }
 
    // Parse rest of the string to Transform
    TArray<FString> Components;
    TransformString.ParseIntoArray(Components, TEXT(","), true);
 
    if (Components.Num() >= 3)
    {
        FVector Location;
        FQuat Rotation;
 
        // Convert the first three components to float values for location
        float X = FCString::Atof(*Components[0]);
        float Y = FCString::Atof(*Components[1]);
        float Z = FCString::Atof(*Components[2]);
 
        Location = FVector(X, Y, Z);
 
        if (Components.Num() >= 6)
        {
            // If there are six or more components, try to parse them as rotation values
            float Pitch = FCString::Atof(*Components[3]);
            float Yaw = FCString::Atof(*Components[4]);
            float Roll = FCString::Atof(*Components[5]);
 
            // Convert the Euler angles to a quaternion
            Rotation = FQuat(FRotator(Pitch, Yaw, Roll));
        }
 
        FTransform Transform;
        Transform.SetLocation(Location);
        Transform.SetRotation(Rotation);
 
#if WITH_EDITOR
        UE_LOG(LogTemp, Warning, TEXT("Teleporting Actor with ID: %s to: %s"), *ID, *TransformString);
#endif
 
        // Check if this Actor is Vehicle
        AVehicle* VehiclePtr = Cast<AVehicle>(Actor);
        if (VehiclePtr)
        {
            // Disable vehicle physics so it can be teleported
            VehiclePtr->TogglePhysics(false);
 
            Actor->SetActorTransform(Transform);
 
            // Enable vehicle physics back on after teleporting vehicle
            VehiclePtr->TogglePhysics(true);
        }
        else
        {
            // Else set the Actor transform
            Actor->SetActorTransform(Transform);
 
            // If the Actor is InstancedActor, we need to notify it. See InstancedActor.cpp
            AInstancedActor* InstancedActorPtr = Cast<AInstancedActor>(Actor);
            if (InstancedActorPtr)
            {
                InstancedActorPtr->UpdateTransformPosition();
            }
        }
    }
}
 
void UROSCommands::HandleTryDestroyObjectByID(const FString& Variable)
{
    bool success = IActorInformation::DestroyActorByID(Variable);
 
    FString Message;
    if (success)
    {
        Message = FString::Printf(TEXT("Object with ID %s successfully destroyed."), *Variable);
    }
    else
    {
        Message = FString::Printf(TEXT("Failed to destroy object with ID %s."), *Variable);
    }
 
    SimulatorLog::Log(Message);
}
 
void UROSCommands::HandleDestroyAllSensors(const FString& Variable)
{
    USensorUtilities::DestroyAllSensors(GetGameWorld());
}
 
void UROSCommands::HandleDestroyAllWalkers(const FString& Variable)
{
    UAssetLibrary::DestroyAllWalkers();
}
 
void UROSCommands::HandleDestroyAllVehicles(const FString& Variable)
{
    UWorld* World = GetGameWorld();
    if (!World)
    {
        return;
    }
 
    TArray<AActor*> Vehicles;
    UGameplayStatics::GetAllActorsOfClass(World, AVehicle::StaticClass(), Vehicles);
    for (AActor* Actor : Vehicles)
    {
        AVehicle* Vehicle = Cast<AVehicle>(Actor);
        if (Vehicle)
        {
            Vehicle->Destroy();
        }
    }
}
 
void UROSCommands::HandlePrintMaps(const FString& Variable)
{
    UAgrarsenseSettings* AgrarsenseSettings = GetSettings();
    if (AgrarsenseSettings)
    {
        TArray<FString> Maps = AgrarsenseSettings->GetMapNames();
 
        FString message;
 
        for (const FString& map : Maps)
        {
            message += FString::Printf(TEXT("%s, "), *map);
        }
 
        SimulatorLog::Log(message);
    }
}
 
void UROSCommands::HandlePrintAllSensors(const FString& Variable)
{
    PrintActorInformation(Variable, ASensor::StaticClass(), "sensor");
}
 
void UROSCommands::HandlePrintAllVehicles(const FString& Variable)
{
    PrintActorInformation(Variable, AVehicle::StaticClass(), "vehicle");
}
 
void UROSCommands::HandlePrintAllWalkers(const FString& Variable)
{
    PrintActorInformation(Variable, AWalker::StaticClass(), "walker");
}
 
void UROSCommands::PrintActorInformation(const FString& Variable, UClass* ActorClass, const FString& ActorTypeName)
{
    TArray<AActor*> Actors;
    UGameplayStatics::GetAllActorsWithInterface(GetGameWorld(), UActorInformation::StaticClass(), Actors);
 
    TArray<AActor*> TypedActors;
    for (AActor* Actor : Actors)
    {
        if (Actor->IsA(ActorClass))
        {
            TypedActors.Add(Actor);
        }
    }
 
    FString Msg;
    if (!TypedActors.IsEmpty())
    {
        Msg = FString::Printf(TEXT("Simulation has %d %ss \n "), TypedActors.Num(), *ActorTypeName);
        for (AActor* Actor : TypedActors)
        {
            if (Actor)
            {
                if (Actor->Implements<UActorInformation>())
                {
                    FString ActorInfo = IActorInformation::Execute_GetActorInformation(Actor);
                    Msg += FString::Printf(TEXT("%s \n \n "), *ActorInfo);
                }
 
            }
        }
    }
    else
    {
        Msg = FString::Printf(TEXT("Simulation has 0 %ss."), *ActorTypeName);
    }
 
    SimulatorLog::Log(Msg);
}
 
void UROSCommands::HandlePrintIds(const FString& Variable)
{
    IActorInformation::PrintAllIds();
}
 
void UROSCommands::HandleSetGlobalTargetFrameRate(const FString& Variable)
{
    if (Variable.IsNumeric())
    {
        UAgrarsenseSettings* AgrarsenseSettings = GetSettings();
        if (AgrarsenseSettings)
        {
            int32 IntValue = FCString::Atoi(*Variable);
            AgrarsenseSettings->SetGlobalTargetFrameRate(IntValue);
        }
    }
}
 
void UROSCommands::HandleSetGlobalTimeDilation(const FString& Variable)
{
    if (Variable.IsNumeric())
    {
        UAgrarsenseSettings* AgrarsenseSettings = GetSettings();
        if (AgrarsenseSettings)
        {
            float FloatValue = FCString::Atof(*Variable);
            AgrarsenseSettings->SetGlobalTimeDilation(FloatValue);
        }
    }
}
 
void UROSCommands::HandleSetQualityLevel(const FString& Variable)
{
    UAgrarsenseSettings* AgrarsenseSettings = GetSettings();
    if (AgrarsenseSettings)
    {
        AgrarsenseSettings->SetQualityLevelFromString(Variable);
    }
}
 
void UROSCommands::HandleSetWorldRendering(const FString& Variable)
{
    bool RenderWorld;
    if (!TryParseBoolean(Variable, RenderWorld))
    {
        // Failed to parse, return
        return;
    }
 
    UAgrarsenseSettings* AgrarsenseSettings = GetSettings();
    if (AgrarsenseSettings)
    {
        AgrarsenseSettings->SetWorldRendering(RenderWorld);
    }
}
 
void UROSCommands::HandleSetSaveCombinedPointcloudToDisk(const FString& Variable)
{
    bool Save;
    if (!TryParseBoolean(Variable, Save))
    {
        // Failed to parse, return
        return;
    }
 
    ALidarManager* LidarManager = UAgrarsenseStatics::GetLidarManager(GetGameWorld());
    if (LidarManager)
    {
        LidarManager->SetSaveCombinedCloudToDisk(Save);
    }
}
 
void UROSCommands::HandleSpawnObjects(const FString& Variable)
{
    USimulatorJsonParser::ParseAndOperateJSONFile(Variable);
}
 
void UROSCommands::HandleExportAll(const FString& Variable)
{
    // Below export functions do not need any incoming variable but require FString
    FString EmptyString;
 
    HandleExportWeather(EmptyString);
    HandleExportWalkers(EmptyString);
 
    // HandleExportSensors exports vehicles as well
    //HandleExportVehicles(EmptyString); 
    HandleExportSensors(EmptyString);
 
    HandleExportFoliage(EmptyString);
    HandleExportProps(EmptyString);
}
 
void UROSCommands::HandleExportWeather(const FString& Variable)
{
    AWeather* Weather = UAgrarsenseStatics::GetWeatherActor(GetGameWorld());
    if (Weather)
    {
        Weather->ExportToJSON("ExportedWeather");
    }
}
 
void UROSCommands::HandleExportWalkers(const FString& Variable)
{
    TArray<AWalker*> Walkers = UAssetLibrary::GetAllWalkers();
    for (AWalker* Walker : Walkers)
    {
        if (Walker)
        {
            Walker->ExportToJsonFile("ExportedWalker");
        }
    }
}
 
void UROSCommands::HandleExportVehicles(const FString& Variable)
{
    TArray<FVehicleData> Vehicles = UAssetLibrary::GetSpawnedVehicles();
    for (const FVehicleData& VehicleData : Vehicles)
    {
        AVehicle* VehiclePtr = VehicleData.Vehicle.Get();
        if (VehiclePtr)
        {
            VehiclePtr->ExportToJsonFile("ExportedVehicle");
        }
    }
}
 
void UROSCommands::HandleExportSensors(const FString& Variable)
{
    UWorld* World = GetGameWorld();
    if (!World)
    {
        return;
    }
 
    // Export all Vehicles and attached sensors
    TArray<AActor*> Vehicles;
    UGameplayStatics::GetAllActorsOfClass(World, AVehicle::StaticClass(), Vehicles);
    for (AActor* Actor : Vehicles)
    {
        AVehicle* VehiclePtr = Cast<AVehicle>(Actor);
        if (VehiclePtr)
        {
            VehiclePtr->ExportToJsonFile("ExportedVehicle");
        }
    }
 
    // Export all Sensors separately as well
    TArray<AActor*> Sensors;
    UGameplayStatics::GetAllActorsOfClass(World, ASensor::StaticClass(), Sensors);
    for (AActor* Actor : Sensors)
    {
        ASensor* SensorPtr = Cast<ASensor>(Actor);
        if (SensorPtr)
        {
            SensorPtr->ExportToJsonFile("ExportedSensor");
        }
    }
}
 
void UROSCommands::HandleExportFoliage(const FString& Variable)
{
    TArray<AInstancedActor*> FoliageActors = UAssetLibrary::GetAllAddedFoliageActors();
    USimulatorJsonExporter::ExportInstancedActorsToJSON("ExportedFoliage", FoliageActors);
}
 
void UROSCommands::HandleExportProps(const FString& Variable)
{
    TArray<AInstancedActor*> PropActors = UAssetLibrary::GetAllAddedPropActors();
    USimulatorJsonExporter::ExportInstancedActorsToJSON("ExportedProps", PropActors);
}
 
void UROSCommands::HandleVisualizeOverlapSensorsBounds(const FString& Variable)
{
    bool Visualize;
    if (!TryParseBoolean(Variable, Visualize))
    {
        // Failed to parse, return
        return;
    }
 
    UWorld* World = GetGameWorld();
    if (World)
    {
        TArray<AActor*> OverlapSensors;
        UGameplayStatics::GetAllActorsOfClass(World, AOverlapSensor::StaticClass(), OverlapSensors);
 
        for (AActor* SensorActor : OverlapSensors)
        {
            AOverlapSensor* Sensor = Cast<AOverlapSensor>(SensorActor);
            if (Sensor)
            {
                Sensor->SetVisualizeOverlapArea(Visualize);
            }
        }
    }
}
 
void UROSCommands::HandleChangeOverlapSensorBounds(const FString& Variable)
{
    FString ID;
    FString BoundsString;
 
    TArray<FString> Tokens;
    Variable.ParseIntoArray(Tokens, TEXT(" "), true);
 
    if (Tokens.Num() < 2)
    {
        // Incoming parameters were invalid, return
        return;
    }
 
    ID = Tokens[0];
    BoundsString = Tokens[1];
 
    FVector Bounds;
    TArray<FString> BoundsTokens;
    BoundsString.ParseIntoArray(BoundsTokens, TEXT(","), true);
 
    if (BoundsTokens.Num() >= 3)
    {
        Bounds.X = FCString::Atof(*BoundsTokens[0]);
        Bounds.Y = FCString::Atof(*BoundsTokens[1]);
        Bounds.Z = FCString::Atof(*BoundsTokens[2]);
 
        AActor* Actor = IActorInformation::GetActorByID(ID);
        AOverlapSensor* Sensor = Cast<AOverlapSensor>(Actor);
        if (Sensor)
        {
            Sensor->SetOverlapBounds(Bounds);
        }
    }
}
 
void UROSCommands::HandleChangeOverlapSensorPosition(const FString& Variable)
{
    FString ID;
    FString BoundsString;
 
    TArray<FString> Tokens;
    Variable.ParseIntoArray(Tokens, TEXT(" "), true);
 
    if (Tokens.Num() < 2)
    {
        // Incoming parameters were invalid, return
        return;
    }
 
    ID = Tokens[0];
    BoundsString = Tokens[1];
 
    FVector RelativePosition;
    TArray<FString> BoundsTokens;
    BoundsString.ParseIntoArray(BoundsTokens, TEXT(","), true);
 
    if (BoundsTokens.Num() >= 3)
    {
        RelativePosition.X = FCString::Atof(*BoundsTokens[0]);
        RelativePosition.Y = FCString::Atof(*BoundsTokens[1]);
        RelativePosition.Z = FCString::Atof(*BoundsTokens[2]);
 
        AActor* Actor = IActorInformation::GetActorByID(ID);
        AOverlapSensor* Sensor = Cast<AOverlapSensor>(Actor);
        if (Sensor)
        {
            Sensor->SetOverlapRelativePosition(RelativePosition);
        }
    }
}
 
void UROSCommands::HandleMoveWalkerToAndDestroy(const FString& Variable)
{
    HandleMoveWalkerTo(Variable, EWalkerEndAction::Destroy);
}
 
void UROSCommands::HandleMoveWalkerToAndStop(const FString& Variable)
{
    HandleMoveWalkerTo(Variable, EWalkerEndAction::Stop);
}
 
void UROSCommands::HandleMoveDroneToAndStop(const FString& Variable)
{
    HandleMoveDroneTo(Variable, EDroneEndAction::Stop);
}
 
void UROSCommands::HandleMoveWalkerTo(const FString& Variable, EWalkerEndAction EndAction)
{
    // This function overrides the current parameters of the Walker identified by the given ID,
    // moving the Walker to the specified x,y,z position and stopping upon reaching the target.
    // Usage: "movewalkerto reindeer 0,0,0" where 'reindeer' is the Walker ID.
 
    FString ID;
    FString TransformString;
 
    // Split the ID and rest of the message
    TArray<FString> Tokens;
    Variable.ParseIntoArray(Tokens, TEXT(" "), true);
    if (Tokens.Num() >= 2)
    {
        ID = Tokens[0];
        TransformString = Variable.RightChop(ID.Len() + 1);
    }
    else
    {
        return;
    }
 
    // Check if this ID matches any Actor
    AActor* Actor = IActorInformation::GetActorByID(ID);
    if (!Actor)
    {
        return;
    }
 
    AWalker* Walker = Cast<AWalker>(Actor);
    if (!Walker)
    {
        return;
    }
 
    // Parse rest of the string to Transform
    TArray<FString> Components;
    TransformString.ParseIntoArray(Components, TEXT(","), true);
 
    if (Components.Num() >= 3)
    {
        FVector Location;
        FQuat Rotation;
 
        // Convert the first three components to float values for location
        float X = FCString::Atof(*Components[0]);
        float Y = FCString::Atof(*Components[1]);
        float Z = FCString::Atof(*Components[2]);
 
        Location = FVector(X, Y, Z);
 
        if (Components.Num() >= 6)
        {
            // If there are six or more components, try to parse them as rotation values
            float Pitch = FCString::Atof(*Components[3]);
            float Yaw = FCString::Atof(*Components[4]);
            float Roll = FCString::Atof(*Components[5]);
 
            // Convert the Euler angles to a quaternion
            Rotation = FQuat(FRotator(Pitch, Yaw, Roll));
        }
 
        FTransform Transform;
        Transform.SetLocation(Location);
        Transform.SetRotation(Rotation);
 
        // Create new walker parameters
        FWalkerParameters NewParams;
        NewParams.WalkerAction = EWalkerAction::FollowPath;
        NewParams.WalkerEndAction = EndAction;
        NewParams.Points.Add(Transform);
 
        // Change Walker parameters
        Walker->ChangeWalkerParameters(NewParams);
    }
}
 
void UROSCommands::HandleStopWalker(const FString& Variable)
{
    HandleSetWalkerMovement(Variable, true);
}
 
void UROSCommands::HandleResumeWalker(const FString& Variable)
{
    HandleSetWalkerMovement(Variable, false);
}
 
void UROSCommands::HandleSetWalkerMovement(const FString& Variable, bool IsPaused)
{
    AActor* Actor = IActorInformation::GetActorByID(Variable);
    AWalker* Walker = Cast<AWalker>(Actor);
    if (!Walker)
    {
        return;
    }
 
    if (IsPaused) 
    {
        Walker->PauseWalker();
    }
    else 
    {
        Walker->ResumeWalker();
    }
}
 
void UROSCommands::HandleMoveDroneTo(const FString& Variable, EDroneEndAction EndAction)
{
    // This function overrides the current parameters of the Drone identified by the given ID,
    // moving the Drone to the specified x,y,z position and stopping upon reaching the target.
    // Usage: "movedroneto drone 0,0,0" where 'drone' is the Drone ID.
 
    FString ID;
    FString TransformString;
 
    // Split the ID and rest of the message
    TArray<FString> Tokens;
    Variable.ParseIntoArray(Tokens, TEXT(" "), true);
    if (Tokens.Num() >= 2)
    {
        ID = Tokens[0];
        TransformString = Variable.RightChop(ID.Len() + 1);
    }
    else
    {
        return;
    }
 
    // Check if this ID matches any Actor
    AActor* Actor = IActorInformation::GetActorByID(ID);
    if (!Actor)
    {
        return;
    }
 
    APIDDrone* Drone = Cast<APIDDrone>(Actor);
    if (!Drone)
    {
        return;
    }
 
    // Parse rest of the string to Transform
    TArray<FString> Components;
    TransformString.ParseIntoArray(Components, TEXT(","), true);
 
    if (Components.Num() >= 3)
    {
        FVector Location;
        FQuat Rotation;
 
        // Convert the first three components to float values for location
        float X = FCString::Atof(*Components[0]);
        float Y = FCString::Atof(*Components[1]);
        float Z = FCString::Atof(*Components[2]);
 
        Location = FVector(X, Y, Z);
 
        if (Components.Num() >= 6)
        {
            // If there are six or more components, try to parse them as rotation values
            float Pitch = FCString::Atof(*Components[3]);
            float Yaw = FCString::Atof(*Components[4]);
            float Roll = FCString::Atof(*Components[5]);
 
            // Convert the Euler angles to a quaternion
            Rotation = FQuat(FRotator(Pitch, Yaw, Roll));
        }
 
        FTransform Transform;
        Transform.SetLocation(Location);
        Transform.SetRotation(Rotation);
 
        // Create new drone parameters
        FDroneParameters NewParams;
        NewParams.DroneAction = EDroneAction::Roaming;
        NewParams.DroneEndAction = EndAction;
        //NewParams.Points.Add(Transform);
 
        Drone->ChangeDroneParameters(NewParams);
 
        // Change drone parameters
        Drone->MoveDroneToPosition(Transform);
    }
}
 
void UROSCommands::HandleSetAllSensorsEnabled(const FString& Variable)
{
    bool EnableSensors;
    if (!TryParseBoolean(Variable, EnableSensors))
    {
        // Failed to parse, return
        return;
    }
 
    if (EnableSensors)
    {
        USensorUtilities::EnableAllSensors(GetGameWorld());
    }
    else 
    {
        USensorUtilities::DisableAllSensors(GetGameWorld());
    }
}
 
void UROSCommands::HandleEnableAllSensors(const FString& Variable)
{
    USensorUtilities::EnableAllSensors(GetGameWorld());
}
 
void UROSCommands::HandleDisableAllSensors(const FString& Variable)
{
    USensorUtilities::DisableAllSensors(GetGameWorld());
}
 
void UROSCommands::HandleSetSensorEnabled(const FString& Variable)
{
    TArray<FString> Tokens;
    Variable.ParseIntoArray(Tokens, TEXT(" "), true);
    if (Tokens.Num() < 2)
    {
        // Not enough arguments, return
        return;
    }
 
    const FString& ID = Tokens[0];
 
    AActor* Actor = IActorInformation::GetActorByID(ID);
    if (!Actor)
    {
        return;
    }
 
    ASensor* Sensor = Cast<ASensor>(Actor);
    if (!Sensor)
    {
        return;
    }
 
    FString BooleanString = Tokens[1].ToLower();
 
    bool SimulateSensor;
    if (!TryParseBoolean(BooleanString, SimulateSensor))
    {
        // Failed to parse, return
        return;
    }
 
    Sensor->SetSimulateSensor(SimulateSensor);
}
 
void UROSCommands::HandleSetWPO(const FString& Variable)
{
    if (Variable.IsNumeric())
    {
        UAgrarsenseSettings* AgrarsenseSettings = GetSettings();
        if (AgrarsenseSettings)
        {
            int32 IntValue = FCString::Atoi(*Variable);
            AgrarsenseSettings->SetWorldPositionOffsetRenderDistance(IntValue);
        }
    }
}
 
void UROSCommands::HandleSetNaniteMaxPixelsPerEdge(const FString& Variable)
{
    if (Variable.IsNumeric())
    {
        UAgrarsenseSettings* AgrarsenseSettings = GetSettings();
        if (AgrarsenseSettings)
        {
            float Value = FCString::Atof(*Variable);
            AgrarsenseSettings->SetNaniteMaxPixelsPerEdge(Value);
        }
    }
}
 
void UROSCommands::HandleDestroyTreesArea(const FString& Variable)
{
    HandleDestroyActors(Variable, true);
}
 
void UROSCommands::HandleDestroyActorsArea(const FString& Variable)
{
    HandleDestroyActors(Variable, false);
}
 
void UROSCommands::HandleDestroyActors(const FString& Variable, bool OnlyTrees)
{
    TArray<FString> ParsedStrings;
    Variable.ParseIntoArray(ParsedStrings, TEXT(" "), true);
 
    if (ParsedStrings.Num() != 2)
    {
        return;
    }
 
    // Parse position
    TArray<FString> PositionComponents;
    ParsedStrings[0].ParseIntoArray(PositionComponents, TEXT(","), true);
 
    if (PositionComponents.Num() != 3)
    {
        return;
    }
 
    FVector Position(FCString::Atof(*PositionComponents[0]), FCString::Atof(*PositionComponents[1]), FCString::Atof(*PositionComponents[2]));
 
    // Parse bounds
    TArray<FString> BoundsComponents;
    ParsedStrings[1].ParseIntoArray(BoundsComponents, TEXT(","), true);
 
    if (BoundsComponents.Num() != 3)
    {
        return;
    }
 
    FVector Bounds(FCString::Atof(*BoundsComponents[0]), FCString::Atof(*BoundsComponents[1]), FCString::Atof(*BoundsComponents[2]));
 
    FTransform VolumeTransform;
    VolumeTransform.SetLocation(Position);
 
    UWorld* World = GetGameWorld();
    if (World)
    {
        ADeletionVolume* Volume = World->SpawnActor<ADeletionVolume>(ADeletionVolume::StaticClass(), VolumeTransform);
        if (Volume)
        {
            // Change volume bounds
            Volume->ChangeOverlapBounds(Bounds);
 
            // Destroy overlapping actors, 
            // this Actor gets deleted automatically after calling this.
            Volume->DestroyOverlappingActors(OnlyTrees);
        }
    }
}