32#include "ROSIntegration/Classes/ROSIntegrationGameInstance.h"
33#include "ROSIntegration/Public/std_msgs/String.h"
34#include "ROSIntegration/Classes/RI/Topic.h"
36#include "Engine/GameViewportClient.h"
37#include "Engine/Engine.h"
38#include "Engine/World.h"
39#include "Misc/DateTime.h"
40#include "Async/Async.h"
71 UWorld* World =
nullptr;
72 if (GEngine && GEngine->GameViewport)
74 World = GEngine->GameViewport->GetWorld();
111 CommandHandlers.Add(TEXT(
"teleportspectator"),
FCommand(&
UROSCommands::HandleTeleportSpectator, 0.03f, TEXT(
"x,y,z or x,y,z,yaw,pitch,roll")));
112 CommandHandlers.Add(TEXT(
"teleportspectatorheight"),
FCommand(&
UROSCommands::HandleTeleportSpectatorHeight, 0.03f, TEXT(
"x,y,z or x,y,z,yaw,pitch,roll")));
113 CommandHandlers.Add(TEXT(
"teleportbyid"),
FCommand(&
UROSCommands::HandleTeleportActorByID, 0.03f, TEXT(
"ID of Actor, x,y,z or x,y,z,yaw,pitch,roll")));
115 CommandHandlers.Add(TEXT(
"teleportbyidheight"),
FCommand(&
UROSCommands::HandleTeleportActorByIDHeight, 0.05f, TEXT(
"ID of Actor, x,y,z or x,y,z,yaw,pitch,roll")));
231 CommandTopic = NewObject<UTopic>(UTopic::StaticClass());
236 std::function<void(TSharedPtr<FROSBaseMsg>)> SubscribeCallback = [
this](TSharedPtr<FROSBaseMsg> msg) ->
void
238 auto Concrete = StaticCastSharedPtr<ROSMessages::std_msgs::String>(msg);
239 if (Concrete.IsValid())
241 FString message = *(Concrete->_Data);
242 AsyncTask(ENamedThreads::GameThread, [
this, message]()
263 if (Message.FindChar(
' ', SpaceIndex))
266 Command = Message.Left(SpaceIndex).ToLower();
269 Variable = Message.Mid(SpaceIndex + 1);
274 Command = Message.ToLower();
278 FString CommandMsg = FString::Printf(TEXT(
"Incoming [command variable]: %s %s"), *Command, *Variable);
285 double CurrentTime = FPlatformTime::Seconds();
288 if (TimeSinceLastExecution >= CommandInfo->
CooldownTime)
291 (this->*(CommandInfo->
Handler))(Variable);
298 float CoolDownTimeLeft = CommandInfo->
CooldownTime - TimeSinceLastExecution;
299 FString Msg = FString::Printf(TEXT(
"Command '%s' on cooldown. Retry in %.2f sec."), *Command, CoolDownTimeLeft);
305 FString Msg = FString::Printf(TEXT(
"Couldn't find command: '%s'"), *Command);
312 FString LoweredString = String.ToLower();
314 if (LoweredString.Equals(
"true") || LoweredString.Equals(
"1"))
319 else if (LoweredString.Equals(
"false") || LoweredString.Equals(
"0"))
336 FString message =
"Available Simulator ROS commands {COMMAND} {VALUE} || ";
340 const FString& CommandString = CommandPair.Key;
341 const FCommand& CommandInfo = CommandPair.Value;
344 message += FString::Printf(TEXT(
"%s || "), *CommandString);
348 message += FString::Printf(TEXT(
"%s %s || "), *CommandString, *CommandInfo.
DefaultValue);
358 if (GEngine && World)
360 GEngine->Exec(World, TEXT(
"quit force"));
367 if (AgrarsenseSettings)
376 if (AgrarsenseSettings)
385 if (AgrarsenseSettings)
393 int32 FrameCount = 0;
394 if (!Variable.IsNumeric())
399 FrameCount = FCString::Atoi(*Variable);
403 if (AgrarsenseSettings)
413 if (!Variable.IsNumeric())
418 Time = FCString::Atof(*Variable);
422 if (AgrarsenseSettings)
432 if (AgrarsenseSettings)
441 if (GEngine && World)
443 GEngine->Exec(World, *Variable);
449 if (!Variable.IsNumeric())
454 float Radius = FCString::Atof(*Variable);
460 if (
AOverlapSensor* OverlapSensor = Spectator->GetOverlapSensor())
462 OverlapSensor->SetOverlapBounds(FVector(Radius, Radius, Radius));
478 TArray<FString> Components;
479 Variable.ParseIntoArray(Components, TEXT(
","),
true);
481 if (Components.Num() >= 3)
487 float X = FCString::Atof(*Components[0]);
488 float Y = FCString::Atof(*Components[1]);
489 float Z = FCString::Atof(*Components[2]);
491 Location = FVector(X, Y, Z);
493 if (Components.Num() >= 6)
496 float Pitch = FCString::Atof(*Components[3]);
497 float Yaw = FCString::Atof(*Components[4]);
498 float Roll = FCString::Atof(*Components[5]);
501 Rotation = FQuat(FRotator(Pitch, Yaw, Roll));
535 TArray<FString> Parts;
536 Variable.TrimStartAndEnd().ParseIntoArrayWS(Parts);
543 int32 LabelIndex = 0;
544 int32 InstanceID = 0;
546 if (!LexTryParseString(LabelIndex, *Parts[0]) || !LexTryParseString(InstanceID, *Parts[1]))
551 TArray<AActor*> Taggers;
552 UGameplayStatics::GetAllActorsOfClass(
GetGameWorld(), ATagger::StaticClass(), Taggers);
553 ATagger* Tagger = Cast<ATagger>(Taggers[0]);
562 Spectator->TeleportSpectator(Actor->GetActorTransform());
571 FString TransformString;
574 TArray<FString> Tokens;
575 Variable.ParseIntoArray(Tokens, TEXT(
" "),
true);
576 if (Tokens.Num() >= 2)
579 TransformString = Variable.RightChop(ID.Len() + 1);
583 SimulatorLog::Log(
"Invalid input. Input should be in format: ID,X,Y,Z or ID,X,Y,Z,YAW,PITCH,ROLL");
591 FString log = FString::Printf(TEXT(
"Could not find Actor with ID: %s"), *ID);
597 TArray<FString> Components;
598 TransformString.ParseIntoArray(Components, TEXT(
","),
true);
600 if (Components.Num() >= 3)
606 float X = FCString::Atof(*Components[0]);
607 float Y = FCString::Atof(*Components[1]);
608 float Z = FCString::Atof(*Components[2]);
610 Location = FVector(X, Y, Z);
612 if (Components.Num() >= 6)
615 float Pitch = FCString::Atof(*Components[3]);
616 float Yaw = FCString::Atof(*Components[4]);
617 float Roll = FCString::Atof(*Components[5]);
620 Rotation = FQuat(FRotator(Pitch, Yaw, Roll));
633 UE_LOG(LogTemp, Warning, TEXT(
"Teleporting Actor with ID: %s to: %s"), *ID, *TransformString);
637 AVehicle* VehiclePtr = Cast<AVehicle>(Actor);
663 Message = FString::Printf(TEXT(
"Object with ID %s successfully destroyed."), *Variable);
667 Message = FString::Printf(TEXT(
"Failed to destroy object with ID %s."), *Variable);
691 TArray<AActor*> Vehicles;
692 UGameplayStatics::GetAllActorsOfClass(World, AVehicle::StaticClass(), Vehicles);
693 for (AActor* Actor : Vehicles)
705 TArray<FString> Parts;
706 Variable.TrimStartAndEnd().ParseIntoArrayWS(Parts);
713 int32 LabelIndex = 0;
714 int32 InstanceID = 0;
716 if (!LexTryParseString(LabelIndex, *Parts[0]) || !LexTryParseString(InstanceID, *Parts[1]))
721 TArray<AActor*> Taggers;
722 UGameplayStatics::GetAllActorsOfClass(
GetGameWorld(), ATagger::StaticClass(), Taggers);
723 ATagger* Tagger = Cast<ATagger>(Taggers[0]);
738 if (AgrarsenseSettings)
740 TArray<FString> Maps = AgrarsenseSettings->
GetMapNames();
744 for (
const FString& map : Maps)
746 message += FString::Printf(TEXT(
"%s, "), *map);
770 TArray<AActor*> Actors;
771 UGameplayStatics::GetAllActorsWithInterface(
GetGameWorld(), UActorInformation::StaticClass(), Actors);
773 TArray<AActor*> TypedActors;
774 for (AActor* Actor : Actors)
776 if (Actor->IsA(ActorClass))
778 TypedActors.Add(Actor);
783 if (!TypedActors.IsEmpty())
785 Msg = FString::Printf(TEXT(
"Simulation has %d %ss \n "), TypedActors.Num(), *ActorTypeName);
786 for (AActor* Actor : TypedActors)
792 FString ActorInfo = IActorInformation::Execute_GetActorInformation(Actor);
793 Msg += FString::Printf(TEXT(
"%s \n \n "), *ActorInfo);
801 Msg = FString::Printf(TEXT(
"Simulation has 0 %ss."), *ActorTypeName);
814 if (Variable.IsNumeric())
817 if (AgrarsenseSettings)
819 int32 IntValue = FCString::Atoi(*Variable);
827 if (Variable.IsNumeric())
830 if (AgrarsenseSettings)
832 float FloatValue = FCString::Atof(*Variable);
841 if (AgrarsenseSettings)
857 if (AgrarsenseSettings)
873 if (AgrarsenseSettings)
932 Walker->ExportToJsonFile(
"ExportedWalker");
942 AVehicle* VehiclePtr = VehicleData.Vehicle.Get();
959 TArray<AActor*> Vehicles;
960 UGameplayStatics::GetAllActorsOfClass(World, AVehicle::StaticClass(), Vehicles);
961 for (AActor* Actor : Vehicles)
963 AVehicle* VehiclePtr = Cast<AVehicle>(Actor);
972 UGameplayStatics::GetAllActorsOfClass(World, ASensor::StaticClass(),
Sensors);
975 ASensor* SensorPtr = Cast<ASensor>(Actor);
997 TArray<AActor*> Taggers;
998 UGameplayStatics::GetAllActorsOfClass(World, ATagger::StaticClass(), Taggers);
1000 if (
ATagger* Tagger = Cast<ATagger>(Taggers[0]))
1002 Tagger->ExportObjectLocationsToCSV();
1024 TArray<AActor*> OverlapSensors;
1025 UGameplayStatics::GetAllActorsOfClass(World, AOverlapSensor::StaticClass(), OverlapSensors);
1027 for (AActor* SensorActor : OverlapSensors)
1041 FString BoundsString;
1043 TArray<FString> Tokens;
1044 Variable.ParseIntoArray(Tokens, TEXT(
" "),
true);
1046 if (Tokens.Num() < 2)
1053 BoundsString = Tokens[1];
1055 if (BoundsString.IsNumeric())
1057 float Radius = FCString::Atof(*BoundsString);
1071 FString BoundsString;
1073 TArray<FString> Tokens;
1074 Variable.ParseIntoArray(Tokens, TEXT(
" "),
true);
1076 if (Tokens.Num() < 2)
1083 BoundsString = Tokens[1];
1085 FVector RelativePosition;
1086 TArray<FString> BoundsTokens;
1087 BoundsString.ParseIntoArray(BoundsTokens, TEXT(
","),
true);
1089 if (BoundsTokens.Num() >= 3)
1091 RelativePosition.X = FCString::Atof(*BoundsTokens[0]);
1092 RelativePosition.Y = FCString::Atof(*BoundsTokens[1]);
1093 RelativePosition.Z = FCString::Atof(*BoundsTokens[2]);
1126 FString TransformString;
1129 TArray<FString> Tokens;
1130 Variable.ParseIntoArray(Tokens, TEXT(
" "),
true);
1131 if (Tokens.Num() >= 2)
1134 TransformString = Variable.RightChop(ID.Len() + 1);
1155 TArray<FString> Components;
1156 TransformString.ParseIntoArray(Components, TEXT(
","),
true);
1158 if (Components.Num() >= 3)
1164 float X = FCString::Atof(*Components[0]);
1165 float Y = FCString::Atof(*Components[1]);
1166 float Z = FCString::Atof(*Components[2]);
1168 Location = FVector(X, Y, Z);
1170 if (Components.Num() >= 6)
1173 float Pitch = FCString::Atof(*Components[3]);
1174 float Yaw = FCString::Atof(*Components[4]);
1175 float Roll = FCString::Atof(*Components[5]);
1178 Rotation = FQuat(FRotator(Pitch, Yaw, Roll));
1192 Walker->ChangeWalkerParameters(NewParams);
1233 Drone->ResetDroneEvent();
1245 FString TransformString;
1248 TArray<FString> Tokens;
1249 Variable.ParseIntoArray(Tokens, TEXT(
" "),
true);
1250 if (Tokens.Num() >= 2)
1253 TransformString = Variable.RightChop(ID.Len() + 1);
1274 TArray<FString> Components;
1275 TransformString.ParseIntoArray(Components, TEXT(
","),
true);
1277 if (Components.Num() >= 3)
1283 float X = FCString::Atof(*Components[0]);
1284 float Y = FCString::Atof(*Components[1]);
1285 float Z = FCString::Atof(*Components[2]);
1287 Location = FVector(X, Y, Z);
1289 if (Components.Num() >= 6)
1292 float Pitch = FCString::Atof(*Components[3]);
1293 float Yaw = FCString::Atof(*Components[4]);
1294 float Roll = FCString::Atof(*Components[5]);
1297 Rotation = FQuat(FRotator(Pitch, Yaw, Roll));
1310 Drone->ChangeDroneParameters(NewParams);
1348 TArray<FString> Tokens;
1349 Variable.ParseIntoArray(Tokens, TEXT(
" "),
true);
1350 if (Tokens.Num() < 2)
1356 const FString& ID = Tokens[0];
1364 ASensor* Sensor = Cast<ASensor>(Actor);
1370 FString BooleanString = Tokens[1].ToLower();
1372 bool SimulateSensor;
1384 if (Variable.IsNumeric())
1387 if (AgrarsenseSettings)
1389 int32 IntValue = FCString::Atoi(*Variable);
1397 if (Variable.IsNumeric())
1400 if (AgrarsenseSettings)
1402 float Value = FCString::Atof(*Variable);
1420 TArray<FString> ParsedStrings;
1421 Variable.ParseIntoArray(ParsedStrings, TEXT(
" "),
true);
1423 if (ParsedStrings.Num() != 2)
1429 TArray<FString> PositionComponents;
1430 ParsedStrings[0].ParseIntoArray(PositionComponents, TEXT(
","),
true);
1432 if (PositionComponents.Num() != 3)
1437 FVector Position(FCString::Atof(*PositionComponents[0]), FCString::Atof(*PositionComponents[1]), FCString::Atof(*PositionComponents[2]));
1440 TArray<FString> BoundsComponents;
1441 ParsedStrings[1].ParseIntoArray(BoundsComponents, TEXT(
","),
true);
1443 if (BoundsComponents.Num() != 3)
1448 FVector Bounds(FCString::Atof(*BoundsComponents[0]), FCString::Atof(*BoundsComponents[1]), FCString::Atof(*BoundsComponents[2]));
1450 FTransform VolumeTransform;
1451 VolumeTransform.SetLocation(Position);
1471 TArray<FString> Tokens;
1472 Variable.ParseIntoArray(Tokens, TEXT(
" "),
true);
1474 if (Tokens.Num() < 2)
1479 const FString& ID = Tokens[0];
1480 const FString Param = Variable.RightChop(ID.Len() + 1);
1482 bool bVisible =
true;
1492 Vehicle->SetVehicleMeshVisibility(bVisible);
void DestroyOverlappingActors(bool OnlyTrees)
void ChangeOverlapBounds(FVector Bounds)
void SetSaveCombinedCloudToDisk(bool bSaveCombinedCloud)
void SetOverlapRelativePosition(const FVector &Vector)
void SetVisualizeOverlapArea(bool Visualize)
void SetOverlapBounds(const FVector &NewSize)
void SetSimulateSensor(bool SimulateSensor)
FString ExportToJsonFile(const FString &FileName)
void TeleportSpectator(const FTransform &Transform)
AActor * TryGetActorByInstanceID(int32 LabelIndex, int32 InstanceID)
FString ExportToJsonFile(const FString &FileName)
void TogglePhysics(bool isOn)
FString ExportToJSON(const FString &FileName)
static void Log(const FString &Message, bool LogToTextFile=true, bool LogToROS=true)
void SetQualityLevelFromString(const FString &QualityLevelString)
void AdvanceTime(float TimeToAdvance)
void ChangeMapByName(FString MapName)
void AdvanceFrameCount(int32 FramesToAdvance)
TArray< FString > GetMapNames() const
void SetGlobalTargetFrameRate(int32 NewTargetFrameRate)
void SetGlobalTimeDilation(float TimeDilation)
void SetGrassVisibility(bool Visible)
void PauseSimulationEndOfThisFrame()
void SetWorldPositionOffsetRenderDistance(int32 WorldPositionOffsetDistance)
void SetWorldRendering(bool enabled)
void SetNaniteMaxPixelsPerEdge(float MaxPixelsPerEdge=1.0f)
static ASpectator * GetSpectator(const UObject *WorldContextObject)
static UAgrarsenseSettings * GetAgrarsenseSettings()
static AWeather * GetWeatherActor(const UObject *WorldContextObject)
static UROSIntegrationGameInstance * GetROSGameInstance(const UObject *WorldContextObject)
static ALidarManager * GetLidarManager(const UObject *WorldContextObject)
static UROSHandler * GetROSHandle(const UObject *WorldContextObject)
static TArray< AInstancedActor * > GetAllAddedPropActors()
static void DestroyAllWalkers()
static TArray< FVehicleData > GetSpawnedVehicles()
static TArray< AInstancedActor * > GetAllAddedFoliageActors()
static TArray< AWalker * > GetAllWalkers()
static bool AlignTransformFromGroundInMeters(AActor *Actor, FTransform &InTransform)
void HandleTeleportActorByID(const FString &Variable)
void HandleUnrealCommand(const FString &Variable)
void HandleSetGrassVisibility(const FString &Variable)
void HandleChangeOverlapSensorBounds(const FString &Variable)
void HandlePrintAllWalkers(const FString &Variable)
void HandleTeleportSpectator(const FString &Variable)
void HandlePrintAllVehicles(const FString &Variable)
void HandlePrintAllSensors(const FString &Variable)
UROSIntegrationGameInstance * RosInstance
void HandleDestroyActorsArea(const FString &Variable)
void HandleSetWalkerMovement(const FString &Variable, bool IsPaused)
void HandleSetSpectatorOverlapRadius(const FString &Variable)
static TMap< FString, FCommand > CommandHandlers
void HandleAdvanceOneFrame(const FString &Variable)
void HandleUnPauseSimulator(const FString &Variable)
bool TryParseBoolean(const FString &String, bool &OutBool) const
void HandlePrintAvailableCommands(const FString &Variable)
void PrintActorInformation(const FString &Variable, UClass *ActorClass, const FString &ActorTypeName)
void HandleAdvanceTime(const FString &Variable)
void HandleExportAll(const FString &Variable)
void HandleExportSensors(const FString &Variable)
void HandleExportVehicles(const FString &Variable)
void HandleSetSaveCombinedPointcloudToDisk(const FString &Variable)
void HandleTeleportActorByIDHeight(const FString &Variable)
void HandleDisableAllSensors(const FString &Variable)
void HandleSetWPO(const FString &Variable)
void HandleTeleportSpectatorByInstanceID(const FString &Variable)
void HandleChangeMap(const FString &Variable)
void HandleExportWalkers(const FString &Variable)
void HandleResumeWalker(const FString &Variable)
UAgrarsenseSettings * GetSettings()
void HandleTeleportSpectatorHeight(const FString &Variable)
void HandleMoveDroneTo(const FString &Variable, EDroneEndAction EndAction)
void HandleSetWorldRendering(const FString &Variable)
void HandleSetGlobalTimeDilation(const FString &Variable)
void HandleDestroyAllSensors(const FString &Variable)
void ParseIncomingMessage(const FString Message)
void SetupROSCommandTopic()
void HandleMoveWalkerToAndStop(const FString &Variable)
void HandleExportWeather(const FString &Variable)
void HandleExportFoliage(const FString &Variable)
void HandleQuit(const FString &Variable)
void HandleSetAllSensorsEnabled(const FString &Variable)
void HandleVehicleMeshVisibility(const FString &Variable)
void HandleChangeOverlapSensorPosition(const FString &Variable)
void HandleSetSensorEnabled(const FString &Variable)
void HandleTryDestroyObjectByID(const FString &Variable)
void HandleVisualizeOverlapSensorsBounds(const FString &Variable)
void HandleEnableAllSensors(const FString &Variable)
void HandleMoveWalkerTo(const FString &Variable, EWalkerEndAction EndAction)
void HandleDestroyAllVehicles(const FString &Variable)
void HandleStopWalker(const FString &Variable)
void HandleDestroyObjectByInstanceID(const FString &Variable)
void HandleDestroyActors(const FString &Variable, bool OnlyTrees)
void HandleMoveDroneToAndStop(const FString &Variable)
void HandleDestroyAllWalkers(const FString &Variable)
void HandleMoveWalkerToAndDestroy(const FString &Variable)
void HandleSetNaniteMaxPixelsPerEdge(const FString &Variable)
void HandlePrintMaps(const FString &Variable)
void HandleSpawnObjects(const FString &Variable)
void HandleDestroyTreesArea(const FString &Variable)
bool TeleportSpectatorHeight
void HandleSetGlobalTargetFrameRate(const FString &Variable)
void HandlePrintIds(const FString &Variable)
void HandleAdvanceFrames(const FString &Variable)
void HandleSetQualityLevel(const FString &Variable)
void HandlePauseSimulator(const FString &Variable)
void HandleResetDrone(const FString &Variable)
void TryExecuteCommand(const FString &Command)
void HandleExportObjects(const FString &Variable)
void HandleExportProps(const FString &Variable)
void ROSBridgeStateChanged(EROSState state)
FROSDelegate_ROState OnROSStateChanged
static void DestroyAllSensors(const UObject *WorldContextObject, bool DestroyDefaultVehicleSensors=false)
static void EnableAllSensors(const UObject *WorldContextObject)
static void DisableAllSensors(const UObject *WorldContextObject)
static FString ExportInstancedActorsToJSON(FString FileName, const TArray< AInstancedActor * > &Actors)
static void ParseAndOperateJSONFile(const FString &Path)
EDroneEndAction DroneEndAction
EWalkerAction WalkerAction
TArray< FTransform > Points
EWalkerEndAction WalkerEndAction