diff --git a/AirLib/update_mavlibkcom.bat b/AirLib/update_mavlibkcom.bat index d4b1f23a8..eb9c19069 100644 --- a/AirLib/update_mavlibkcom.bat +++ b/AirLib/update_mavlibkcom.bat @@ -1,3 +1,27 @@ -robocopy /MIR ..\MavLinkCom\SDK\Includes deps\MavLinkCom\include /XD temp *. /njh /njs /ndl /np -robocopy /MIR ..\MavLinkCom\SDK\Libraries deps\MavLinkCom\lib /XD temp *. /njh /njs /ndl /np -pause \ No newline at end of file +@echo off +setlocal + +set "MAVLINKCOM_ROOT=..\MavLinkCom" +set "INCLUDE_SRC=%MAVLINKCOM_ROOT%\SDK\Includes" +set "LIB_SRC=%MAVLINKCOM_ROOT%\SDK\Libraries" + +if exist "%MAVLINKCOM_ROOT%\include" set "INCLUDE_SRC=%MAVLINKCOM_ROOT%\include" +if exist "%MAVLINKCOM_ROOT%\lib" set "LIB_SRC=%MAVLINKCOM_ROOT%\lib" + +if not exist "%INCLUDE_SRC%" ( + echo [update_mavlibkcom] Include source not found: %INCLUDE_SRC% + exit /b 1 +) + +if not exist "%LIB_SRC%" ( + echo [update_mavlibkcom] Library source not found: %LIB_SRC% + exit /b 1 +) + +robocopy /MIR "%INCLUDE_SRC%" deps\MavLinkCom\include /XD temp *. /njh /njs /ndl /np +if %errorlevel% geq 8 exit /b %errorlevel% + +robocopy /MIR "%LIB_SRC%" deps\MavLinkCom\lib /XD temp *. /njh /njs /ndl /np +if %errorlevel% geq 8 exit /b %errorlevel% + +exit /b 0 diff --git a/MavLinkCom/common_utils/Utils.hpp b/MavLinkCom/common_utils/Utils.hpp index b21d5c0b3..9488a5515 100644 --- a/MavLinkCom/common_utils/Utils.hpp +++ b/MavLinkCom/common_utils/Utils.hpp @@ -28,6 +28,13 @@ #include // needed for CHAR_BIT used below #endif +#ifdef min +#undef min +#endif +#ifdef max +#undef max +#endif + //Stubs for C++17 optional type #if (defined __cplusplus) && (__cplusplus >= 201700L) #include @@ -80,11 +87,14 @@ __attribute__((__format__(__printf__, 1, 0))) static int _vscprintf(const char* #endif // Call this on a function parameter to suppress the unused paramter warning +#ifndef AIRSIM_UNUSED_HELPER_DEFINED +#define AIRSIM_UNUSED_HELPER_DEFINED template inline void unused(T const& result) { static_cast(result); } +#endif namespace mavlink_utils { diff --git a/Unreal/Plugins/AirSim/AirSim.uplugin b/Unreal/Plugins/AirSim/AirSim.uplugin index 8d0420b2b..86aac01e4 100644 --- a/Unreal/Plugins/AirSim/AirSim.uplugin +++ b/Unreal/Plugins/AirSim/AirSim.uplugin @@ -22,6 +22,10 @@ } ], "Plugins": [ + { + "Name": "Carla", + "Enabled": true + }, { "Name": "PhysXVehicles", "Enabled": true diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index cc8dcb311..cb30e3e21 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -9,6 +9,7 @@ #include "Components/StaticMeshComponent.h" #include "EngineUtils.h" #include "Runtime/Engine/Classes/Engine/StaticMesh.h" +#include "Runtime/Engine/Classes/Engine/LevelStreamingDynamic.h" #include "UObject/UObjectIterator.h" #include "Camera/CameraComponent.h" #include "Runtime/Engine/Classes/GameFramework/PlayerStart.h" @@ -20,6 +21,7 @@ #include "Misc/ObjectThumbnail.h" #include "Engine/Engine.h" #include "Engine/World.h" +#include #include #include "common/common_utils/Utils.hpp" #include "Modules/ModuleManager.h" @@ -34,6 +36,7 @@ Methods -> CamelCase parameters -> camel_case */ +ULevelStreamingDynamic* UAirBlueprintLib::CURRENT_LEVEL = nullptr; bool UAirBlueprintLib::log_messages_hidden_ = false; msr::airlib::AirSimSettings::SegmentationSetting::MeshNamingMethodType UAirBlueprintLib::mesh_naming_method_ = msr::airlib::AirSimSettings::SegmentationSetting::MeshNamingMethodType::OwnerName; @@ -74,23 +77,18 @@ void UAirBlueprintLib::setSimulatePhysics(AActor* actor, bool simulate_physics) } } -bool UAirBlueprintLib::loadLevel(UObject* context, const FString& level_name) +ULevelStreamingDynamic* UAirBlueprintLib::loadLevel(UObject* context, const FString& level_name) { - bool success{ false }; - // Get name of current level - auto currMap = context->GetWorld()->GetMapName(); - currMap.RemoveFromStart(context->GetWorld()->StreamingLevelsPrefix); - // Only load new level if different from current level - if (!currMap.Equals(level_name)) { - FString LongPackageName; - success = FPackageName::SearchForPackageOnDisk(level_name, &LongPackageName); - if (success) { - context->GetWorld()->SetNewWorldOrigin(FIntVector(0, 0, 0)); - UGameplayStatics::OpenLevel(context->GetWorld(), FName(*LongPackageName), true); - } + context->GetWorld()->SetNewWorldOrigin(FIntVector(0, 0, 0)); + ULevelStreamingDynamic* new_level = UAirsimLevelStreaming::LoadAirsimLevelInstance( + context->GetWorld(), level_name, FVector(0, 0, 0), FRotator(0, 0, 0), success); + if (success) { + if (CURRENT_LEVEL != nullptr && CURRENT_LEVEL->IsValidLowLevel()) + CURRENT_LEVEL->SetShouldBeLoaded(false); + CURRENT_LEVEL = new_level; } - return success; + return CURRENT_LEVEL; } bool UAirBlueprintLib::spawnPlayer(UWorld* context) @@ -316,6 +314,7 @@ template USceneCaptureComponent2D* UAirBlueprintLib::GetActorComponent(AActor*, template UStaticMeshComponent* UAirBlueprintLib::GetActorComponent(AActor*, FString); template URotatingMovementComponent* UAirBlueprintLib::GetActorComponent(AActor*, FString); template UCameraComponent* UAirBlueprintLib::GetActorComponent(AActor*, FString); +template UCineCameraComponent* UAirBlueprintLib::GetActorComponent(AActor*, FString); template UDetectionComponent* UAirBlueprintLib::GetActorComponent(AActor*, FString); bool UAirBlueprintLib::IsInGameThread() @@ -458,12 +457,8 @@ std::vector UAirBlueprintLib::Ge //Various checks if there is even a valid mesh if (!comp->GetStaticMesh()) continue; - if (!comp->GetStaticMesh()->HasValidRenderData()) continue; -#if ((ENGINE_MAJOR_VERSION > 4) || (ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION >= 27)) - if (comp->GetStaticMesh()->GetRenderData()->LODResources.Num() == 0) continue; -#else + if (!comp->GetStaticMesh()->RenderData) continue; if (comp->GetStaticMesh()->RenderData->LODResources.Num() == 0) continue; -#endif msr::airlib::MeshPositionVertexBuffersResponse mesh; mesh.name = name; @@ -478,11 +473,7 @@ std::vector UAirBlueprintLib::Ge mesh.orientation.y() = att.Y; mesh.orientation.z() = att.Z; -#if ((ENGINE_MAJOR_VERSION > 4) || (ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION >= 27)) - FPositionVertexBuffer* vertex_buffer = &comp->GetStaticMesh()->GetRenderData()->LODResources[0].VertexBuffers.PositionVertexBuffer; -#else FPositionVertexBuffer* vertex_buffer = &comp->GetStaticMesh()->RenderData->LODResources[0].VertexBuffers.PositionVertexBuffer; -#endif if (vertex_buffer) { const int32 vertex_count = vertex_buffer->VertexBufferRHI->GetSize(); TArray vertices; @@ -497,11 +488,7 @@ std::vector UAirBlueprintLib::Ge RHIUnlockVertexBuffer(vertex_buffer->VertexBufferRHI); }); -#if ((ENGINE_MAJOR_VERSION > 4) || (ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION >= 27)) - FStaticMeshLODResources& lod = comp->GetStaticMesh()->GetRenderData()->LODResources[0]; -#else FStaticMeshLODResources& lod = comp->GetStaticMesh()->RenderData->LODResources[0]; -#endif FRawStaticIndexBuffer* IndexBuffer = &lod.IndexBuffer; int num_indices = IndexBuffer->IndexBufferRHI->GetSize() / IndexBuffer->IndexBufferRHI->GetStride(); diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 46d2a8a93..86bc8b21d 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -19,12 +19,15 @@ #include "Engine/World.h" #include "Runtime/Landscape/Classes/LandscapeComponent.h" #include "Runtime/Engine/Classes/Kismet/GameplayStatics.h" +#include "AirsimLevelStreaming.h" #include "Runtime/Core/Public/HAL/FileManager.h" #include "common/AirSimSettings.hpp" #include #include #include "AirBlueprintLib.generated.h" +class ULevelStreamingDynamic; + UENUM(BlueprintType) enum class LogDebugLevel : uint8 { @@ -71,9 +74,11 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary UGameplayStatics::GetAllActorsOfClass(context, T::StaticClass(), foundActors); } + static ULevelStreamingDynamic* CURRENT_LEVEL; + static std::vector ListMatchingActors(const UObject* context, const std::string& name_regex); UFUNCTION(BlueprintCallable, Category = "AirSim|LevelAPI") - static bool loadLevel(UObject* context, const FString& level_name); + static ULevelStreamingDynamic* loadLevel(UObject* context, const FString& level_name); UFUNCTION(BlueprintCallable, Category = "AirSim|LevelAPI") static bool spawnPlayer(UWorld* context); UFUNCTION(BlueprintPure, Category = "AirSim|LevelAPI") diff --git a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs index 17024a35b..a45de60e1 100644 --- a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs +++ b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs @@ -79,15 +79,17 @@ public AirSim(ReadOnlyTargetRules Target) : base(Target) bEnableExceptions = true; PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore", "ImageWrapper", "RenderCore", "RHI", "AssetRegistry", "PhysicsCore", "PhysXVehicles", "PhysXVehicleLib", "PhysX", "APEX", "Landscape", "CinematicCamera" }); - PrivateDependencyModuleNames.AddRange(new string[] { "UMG", "Slate", "SlateCore" }); + PrivateDependencyModuleNames.AddRange(new string[] { "UMG", "Slate", "SlateCore", "Carla", "Foliage" }); //suppress VC++ proprietary warnings PublicDefinitions.Add("_SCL_SECURE_NO_WARNINGS=1"); PublicDefinitions.Add("_CRT_SECURE_NO_WARNINGS=1"); PublicDefinitions.Add("HMD_MODULE_INCLUDED=0"); + PublicDefinitions.Add("NOMINMAX"); PublicIncludePaths.Add(Path.Combine(AirLibPath, "include")); PublicIncludePaths.Add(Path.Combine(AirLibPath, "deps", "eigen3")); + PublicIncludePaths.Add(Path.Combine(ModulePath, "MavLinkCom", "common_utils")); AddOSLibDependencies(Target); SetupCompileMode(CompileMode.HeaderOnlyWithRpc, Target); diff --git a/Unreal/Plugins/AirSim/Source/AirSim.cpp b/Unreal/Plugins/AirSim/Source/AirSim.cpp index 37d4511d1..8fbbc514b 100644 --- a/Unreal/Plugins/AirSim/Source/AirSim.cpp +++ b/Unreal/Plugins/AirSim/Source/AirSim.cpp @@ -5,6 +5,12 @@ #include "Misc/Paths.h" #include "Modules/ModuleManager.h" #include "Modules/ModuleInterface.h" +#if WITH_EDITOR +#include +#include +#include +#include +#endif class FAirSim : public IModuleInterface { @@ -23,4 +29,17 @@ void FAirSim::StartupModule() void FAirSim::ShutdownModule() { //plugin shutdown -} \ No newline at end of file +} + +#if WITH_EDITOR +namespace carla +{ + +void throw_exception(const std::exception& e) +{ + UE_LOG(LogTemp, Fatal, TEXT("Carla exception forwarded via AirSim: %s"), UTF8_TO_TCHAR(e.what())); + std::terminate(); +} + +} // namespace carla +#endif diff --git a/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.cpp b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.cpp new file mode 100644 index 000000000..f7871d56f --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.cpp @@ -0,0 +1,48 @@ +#pragma once + +#include "AirsimLevelStreaming.h" +#include "Engine/World.h" + +int32 UAirsimLevelStreaming::LevelInstanceId = 0; + +UAirsimLevelStreaming* UAirsimLevelStreaming::LoadAirsimLevelInstance(UWorld* WorldContextObject, FString LevelName, FVector Location, FRotator Rotation, bool& bOutSuccess) +{ + if (!WorldContextObject) { + return nullptr; + } + + // Check whether requested map exists, this could be very slow if LevelName is a short package name + FString LongPackageName; + bool success = FPackageName::SearchForPackageOnDisk(LevelName, &LongPackageName); + if (!success) { + bOutSuccess = false; + return nullptr; + } + + // Create Unique Name for sub-level package + const FString ShortPackageName = FPackageName::GetShortName(LongPackageName); + const FString PackagePath = FPackageName::GetLongPackagePath(LongPackageName); + FString UniqueLevelPackageName = PackagePath + TEXT("/") + WorldContextObject->StreamingLevelsPrefix + ShortPackageName; + UniqueLevelPackageName += TEXT("_LevelInstance_") + FString::FromInt(++LevelInstanceId); + + // Setup streaming level object that will load specified map + ULevelStreamingDynamic* level_pointer = NewObject(WorldContextObject, ULevelStreamingDynamic::StaticClass(), NAME_None, RF_Transient, NULL); + level_pointer->SetWorldAssetByPackageName(FName(*UniqueLevelPackageName)); + level_pointer->LevelColor = FColor::MakeRandomColor(); + level_pointer->SetShouldBeLoaded(true); + level_pointer->SetShouldBeVisible(true); + level_pointer->bShouldBlockOnLoad = true; + level_pointer->bInitiallyLoaded = true; + level_pointer->bInitiallyVisible = true; + + // Transform + level_pointer->LevelTransform = FTransform(Rotation, Location); + // Map to Load + level_pointer->PackageNameToLoad = FName(*LongPackageName); + // Add the new level to world. + WorldContextObject->AddStreamingLevel(level_pointer); + + bOutSuccess = true; + + return dynamic_cast(level_pointer); +} \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.h b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.h new file mode 100644 index 000000000..903587539 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.h @@ -0,0 +1,11 @@ +#pragma once +#include "Runtime/Engine/Classes/Engine/LevelStreamingDynamic.h" + +class UAirsimLevelStreaming : public ULevelStreamingDynamic +{ +public: + static UAirsimLevelStreaming* LoadAirsimLevelInstance(UWorld* WorldContextObject, FString LevelName, FVector Location, FRotator Rotation, bool& bOutSuccess); + +private: + static int32 LevelInstanceId; +}; \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/CameraDirector.cpp b/Unreal/Plugins/AirSim/Source/CameraDirector.cpp index 43f0a3f2a..eabe26d28 100644 --- a/Unreal/Plugins/AirSim/Source/CameraDirector.cpp +++ b/Unreal/Plugins/AirSim/Source/CameraDirector.cpp @@ -29,7 +29,8 @@ void ACameraDirector::Tick(float DeltaTime) Super::Tick(DeltaTime); if (mode_ == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL) { - manual_pose_controller_->updateActorPose(DeltaTime); + if (manual_pose_controller_) + manual_pose_controller_->updateActorPose(DeltaTime); } else if (mode_ == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE) { //do nothing, spring arm is pulling the camera with it @@ -56,6 +57,7 @@ void ACameraDirector::initializeForBeginPlay(ECameraDirectorMode view_mode, setupInputBindings(); mode_ = view_mode; + mode_before_front_ = view_mode; // default restore target = initial view follow_actor_ = follow_actor; fpv_camera_ = fpv_camera; @@ -151,7 +153,7 @@ void ACameraDirector::setMode(ECameraDirectorMode mode) //Remove any existing key bindings for manual mode if (mode != ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL) { - if (ExternalCamera != nullptr && manual_pose_controller_->getActor() == ExternalCamera) { + if (ExternalCamera != nullptr && manual_pose_controller_ && manual_pose_controller_->getActor() == ExternalCamera) { manual_pose_controller_->setActor(nullptr); } @@ -286,13 +288,40 @@ void ACameraDirector::inputEventBackupView() void ACameraDirector::inputEventFrontView() { + // Toggle: if already in Front view → restore the view that was active before pressing I + if (mode_ == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FRONT || mode_ == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FPV) { + // Restore previous mode + switch (mode_before_front_) { + case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_GROUND_OBSERVER: + inputEventGroundView(); + break; + case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL: + inputEventManualView(); + break; + case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE: + inputEventSpringArmChaseView(); + break; + case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_BACKUP: + inputEventBackupView(); + break; + default: + // FLY_WITH_ME or anything else + inputEventFlyWithView(); + break; + } + return; + } + + // Save current mode before switching to front view + mode_before_front_ = mode_; + if (front_camera_) { setMode(ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FRONT); front_camera_->showToScreen(); disableCameras(true, true, true, false); } else - UAirBlueprintLib::LogMessageString("Camera is not available: ", "backup_camera", LogDebugLevel::Failure); + UAirBlueprintLib::LogMessageString("Camera is not available: ", "front_camera", LogDebugLevel::Failure); notifyViewModeChanged(); } diff --git a/Unreal/Plugins/AirSim/Source/CameraDirector.h b/Unreal/Plugins/AirSim/Source/CameraDirector.h index 59f4557ed..bd565d49a 100644 --- a/Unreal/Plugins/AirSim/Source/CameraDirector.h +++ b/Unreal/Plugins/AirSim/Source/CameraDirector.h @@ -82,6 +82,7 @@ class AIRSIM_API ACameraDirector : public AActor USceneComponent* last_parent_ = nullptr; ECameraDirectorMode mode_; + ECameraDirectorMode mode_before_front_; // saved mode before I-key toggle UPROPERTY() UManualPoseController* manual_pose_controller_; diff --git a/Unreal/Plugins/AirSim/Source/DetectionComponent.cpp b/Unreal/Plugins/AirSim/Source/DetectionComponent.cpp index a1fc1281f..a72a9929e 100644 --- a/Unreal/Plugins/AirSim/Source/DetectionComponent.cpp +++ b/Unreal/Plugins/AirSim/Source/DetectionComponent.cpp @@ -142,7 +142,7 @@ bool UDetectionComponent::calcBoundingFromViewInfo(AActor* actor, FBox2D& box_ou for (FVector& point : points) { is_world_hit = GetWorld()->LineTraceSingleByChannel(result, GetComponentLocation(), point, ECC_WorldStatic); if (is_world_hit) { - if (result.GetActor() == actor) { + if (result.Actor == actor) { is_visible = true; break; } @@ -156,7 +156,7 @@ bool UDetectionComponent::calcBoundingFromViewInfo(AActor* actor, FBox2D& box_ou FVector point = UKismetMathLibrary::RandomPointInBoundingBox(origin, extend); is_world_hit = GetWorld()->LineTraceSingleByChannel(result, GetComponentLocation(), point, ECC_WorldStatic); if (is_world_hit) { - if (result.GetActor() == actor) { + if (result.Actor == actor) { is_visible = true; break; } diff --git a/Unreal/Plugins/AirSim/Source/ObjectFilter.cpp b/Unreal/Plugins/AirSim/Source/ObjectFilter.cpp index 25c48b4f9..291dc324e 100644 --- a/Unreal/Plugins/AirSim/Source/ObjectFilter.cpp +++ b/Unreal/Plugins/AirSim/Source/ObjectFilter.cpp @@ -53,7 +53,6 @@ bool FObjectFilter::matchesActor(AActor* actor) const return true; } if (wildcard_mesh_names_.Num() != 0 && - skeletal_mesh_component->SkeletalMesh && isMatchAnyWildcard(skeletal_mesh_component->SkeletalMesh->GetName())) { return true; } diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp index 114c08f31..29a12cdcd 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp @@ -36,16 +36,16 @@ APIPCamera::APIPCamera(const FObjectInitializer& ObjectInitializer) PrimaryActorTick.bCanEverTick = true; - image_type_to_pixel_format_map_.Add(Utils::toNumeric(ImageType::Scene), EPixelFormat::PF_B8G8R8A8); - image_type_to_pixel_format_map_.Add(Utils::toNumeric(ImageType::DepthPlanar), EPixelFormat::PF_DepthStencil); // not used. init_auto_format is called in setupCameraFromSettings() - image_type_to_pixel_format_map_.Add(Utils::toNumeric(ImageType::DepthPerspective), EPixelFormat::PF_DepthStencil); // not used for same reason as above - image_type_to_pixel_format_map_.Add(Utils::toNumeric(ImageType::DepthVis), EPixelFormat::PF_DepthStencil); // not used for same reason as above - image_type_to_pixel_format_map_.Add(Utils::toNumeric(ImageType::DisparityNormalized), EPixelFormat::PF_DepthStencil); // not used for same reason as above - image_type_to_pixel_format_map_.Add(Utils::toNumeric(ImageType::Segmentation), EPixelFormat::PF_B8G8R8A8); - image_type_to_pixel_format_map_.Add(Utils::toNumeric(ImageType::SurfaceNormals), EPixelFormat::PF_B8G8R8A8); - image_type_to_pixel_format_map_.Add(Utils::toNumeric(ImageType::Infrared), EPixelFormat::PF_B8G8R8A8); - image_type_to_pixel_format_map_.Add(Utils::toNumeric(ImageType::OpticalFlow), EPixelFormat::PF_B8G8R8A8); - image_type_to_pixel_format_map_.Add(Utils::toNumeric(ImageType::OpticalFlowVis), EPixelFormat::PF_B8G8R8A8); + image_type_to_pixel_format_map_.Add(0, EPixelFormat::PF_B8G8R8A8); + image_type_to_pixel_format_map_.Add(1, EPixelFormat::PF_DepthStencil); // not used. init_auto_format is called in setupCameraFromSettings() + image_type_to_pixel_format_map_.Add(2, EPixelFormat::PF_DepthStencil); // not used for same reason as above + image_type_to_pixel_format_map_.Add(3, EPixelFormat::PF_DepthStencil); // not used for same reason as above + image_type_to_pixel_format_map_.Add(4, EPixelFormat::PF_DepthStencil); // not used for same reason as above + image_type_to_pixel_format_map_.Add(5, EPixelFormat::PF_B8G8R8A8); + image_type_to_pixel_format_map_.Add(6, EPixelFormat::PF_B8G8R8A8); + image_type_to_pixel_format_map_.Add(7, EPixelFormat::PF_B8G8R8A8); + image_type_to_pixel_format_map_.Add(8, EPixelFormat::PF_B8G8R8A8); + image_type_to_pixel_format_map_.Add(9, EPixelFormat::PF_B8G8R8A8); object_filter_ = FObjectFilter(); } @@ -382,26 +382,19 @@ void APIPCamera::setupCameraFromSettings(const APIPCamera::CameraSetting& camera const auto& noise_setting = camera_setting.noise_settings.at(image_type); if (image_type >= 0) { //scene capture components - auto pixel_format_override = camera_setting.ue_setting.pixel_format_override_settings.find(image_type); - EPixelFormat pixel_format = EPixelFormat::PF_Unknown; - if (pixel_format_override != camera_setting.ue_setting.pixel_format_override_settings.end()) { - pixel_format = static_cast(pixel_format_override->second.pixel_format); - } - pixel_format = (pixel_format == EPixelFormat::PF_Unknown ? image_type_to_pixel_format_map_[image_type] : pixel_format); - switch (Utils::toEnum(image_type)) { case ImageType::Scene: case ImageType::Infrared: - updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], false, pixel_format, capture_setting, ned_transform, false); + updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], false, image_type_to_pixel_format_map_[image_type], capture_setting, ned_transform, false); break; case ImageType::Segmentation: case ImageType::SurfaceNormals: - updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], true, pixel_format, capture_setting, ned_transform, true); + updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], true, image_type_to_pixel_format_map_[image_type], capture_setting, ned_transform, true); break; default: - updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], true, pixel_format, capture_setting, ned_transform, false); + updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], true, image_type_to_pixel_format_map_[image_type], capture_setting, ned_transform, false); break; } setDistortionMaterial(image_type, captures_[image_type], captures_[image_type]->PostProcessSettings); @@ -778,6 +771,7 @@ void APIPCamera::copyCameraSettingsToSceneCapture(UCameraComponent* src, USceneC { if (src && dst) { dst->SetWorldLocationAndRotation(src->GetComponentLocation(), src->GetComponentRotation()); + dst->FOVAngle = src->FieldOfView; FMinimalViewInfo camera_view_info; src->GetCameraView(/*DeltaTime =*/0.0f, camera_view_info); diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index 75a689e94..4bcb4c92e 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -108,6 +108,7 @@ void PawnSimApi::setupCamerasFromSettings(const common_utils::UniqueValueMapcameras, pair.first, camera_defaults); diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index 0c622dc8f..4e24035b8 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.h @@ -170,6 +170,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase mutable SimJoyStick joystick_; mutable SimJoyStick::State joystick_state_; +protected: //state (accessible by subclasses for manual collision injection) struct State { FVector start_location; @@ -193,6 +194,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase State state_, initial_state_; +private: std::unique_ptr kinematics_; std::unique_ptr environment_; diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index 147e9ec8e..5b8f8ba53 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -22,12 +22,9 @@ void ASimHUD::BeginPlay() try { UAirBlueprintLib::OnBeginPlay(); - initializeSettings(); loadLevel(); - // Prevent a MavLink connection being established if changing levels - if (map_changed_) return; - + initializeSettings(); setUnrealEngineSettings(); createSimMode(); createMainWidget(); @@ -256,7 +253,10 @@ std::string ASimHUD::getSimModeFromUser() void ASimHUD::loadLevel() { - UAirBlueprintLib::RunCommandOnGameThread([&]() { this->map_changed_ = UAirBlueprintLib::loadLevel(this->GetWorld(), FString(AirSimSettings::singleton().level_name.c_str())); }, true); + if (AirSimSettings::singleton().level_name != "") + UAirBlueprintLib::RunCommandOnGameThread([&]() { UAirBlueprintLib::loadLevel(this->GetWorld(), FString(AirSimSettings::singleton().level_name.c_str())); }, true); + else + UAirBlueprintLib::RunCommandOnGameThread([&]() { UAirBlueprintLib::loadLevel(this->GetWorld(), FString("Blocks")); }, true); } void ASimHUD::createSimMode() @@ -344,21 +344,27 @@ bool ASimHUD::getSettingsText(std::string& settingsText) // Returns true if the argument is present, false otherwise. bool ASimHUD::getSettingsTextFromCommandLine(std::string& settingsText) { + + bool found = false; + FString settingsTextFString; const TCHAR* commandLineArgs = FCommandLine::Get(); - FString settingsJsonFString; - if (FParse::Value(commandLineArgs, TEXT("-settings="), settingsJsonFString, false)) { - if (readSettingsTextFromFile(settingsJsonFString, settingsText)) { + if (FParse::Param(commandLineArgs, TEXT("-settings"))) { + FString commandLineArgsFString = FString(commandLineArgs); + int idx = commandLineArgsFString.Find(TEXT("-settings")); + FString settingsJsonFString = commandLineArgsFString.RightChop(idx + 10); + + if (readSettingsTextFromFile(settingsJsonFString.TrimQuotes(), settingsText)) { return true; } - else { - UAirBlueprintLib::LogMessageString("Loaded settings from commandline: ", TCHAR_TO_UTF8(*settingsJsonFString), LogDebugLevel::Informational); - settingsText = TCHAR_TO_UTF8(*settingsJsonFString); - return true; + + if (FParse::QuotedString(*settingsJsonFString, settingsTextFString)) { + settingsText = std::string(TCHAR_TO_UTF8(*settingsTextFString)); + found = true; } } - return false; + return found; } bool ASimHUD::readSettingsTextFromFile(const FString& settingsFilepath, std::string& settingsText) diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp index 83568ec70..1e33a158a 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp @@ -17,6 +17,7 @@ #include "common/common_utils/WindowsApisCommonPre.hpp" #include "common/common_utils/MinWinDefines.hpp" +#include #include #include diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 6edb12386..95b491e0b 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -21,6 +21,13 @@ #include "Weather/WeatherLib.h" #include "DrawDebugHelpers.h" +#include "Engine/StaticMeshActor.h" +#include "Components/DirectionalLightComponent.h" +#include "Components/SkyLightComponent.h" +#include "Components/PostProcessComponent.h" +#include "Engine/DirectionalLight.h" +#include "Engine/ExponentialHeightFog.h" +#include "Components/ExponentialHeightFogComponent.h" //TODO: this is going to cause circular references which is fine here but //in future we should consider moving SimMode not derived from AActor and move @@ -110,11 +117,17 @@ void ASimModeBase::BeginPlay() } player_start_transform = fpv_pawn->GetActorTransform(); player_loc = player_start_transform.GetLocation(); - // Move the world origin to the player's location (this moves the coordinate system and adds - // a corresponding offset to all positions to compensate for the shift) - this->GetWorld()->SetNewWorldOrigin(FIntVector(player_loc) + this->GetWorld()->OriginLocation); - // Regrab the player's position after the offset has been added (which should be 0,0,0 now) - player_start_transform = fpv_pawn->GetActorTransform(); + // NOTE: Do NOT call SetNewWorldOrigin() here. In CarlaAir (CARLA + AirSim integration), + // shifting the world origin breaks CARLA's landscape/road collision, causing all ground + // vehicles to fall through the map (~30m underground). The NedTransform is initialized + // with the original player_start_transform, which correctly serves as the NED origin + // for AirSim coordinate conversion without moving the world. + // + // KNOWN LIMITATION: In CARLA maps, PlayerStart is typically 20-30m above ground level. + // The drone spawns at NED origin (PlayerStart) and freefalls to the ground. SimpleFlight's + // ground lock mechanism prevents takeoff from ground contact. Python scripts should use + // simSetVehiclePose() to teleport the drone above ground, then moveToZAsync() to hover, + // instead of takeoffAsync(). global_ned_transform_.reset(new NedTransform(player_start_transform, UAirBlueprintLib::GetWorldToMetersScale(this))); @@ -134,6 +147,170 @@ void ASimModeBase::BeginPlay() record_tick_count = 0; setupInputBindings(); + // Note: CARLA RPC server initialization is handled by SimWorldGameMode (via + // ACarlaGameModeBase::InitGame -> GameInstance->NotifyInitGame()). + + // Ensure basic environment exists for maps that lack content (e.g. AirSimAssets) + { + UWorld* World = GetWorld(); + UStaticMesh* cube_mesh = LoadObject(nullptr, TEXT("/Engine/BasicShapes/Cube.Cube")); + UMaterial* base_mat = LoadObject(nullptr, TEXT("/Engine/BasicShapes/BasicShapeMaterial.BasicShapeMaterial")); + + // Check if map has real content (static mesh actors) + TArray existing_meshes; + UGameplayStatics::GetAllActorsOfClass(World, AStaticMeshActor::StaticClass(), existing_meshes); + if (existing_meshes.Num() == 0 && cube_mesh) { + UE_LOG(LogTemp, Warning, TEXT("AirSim: Empty map detected - spawning test environment")); + + // Remove existing ExponentialHeightFog (from WeatherActor, causes black fog) + TArray fog_actors; + UGameplayStatics::GetAllActorsOfClass(World, AExponentialHeightFog::StaticClass(), fog_actors); + for (AActor* fog : fog_actors) { + fog->Destroy(); + } + + FActorSpawnParameters sp; + sp.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AlwaysSpawn; + + // --- Sun (directional light) --- + TArray existing_lights; + UGameplayStatics::GetAllActorsOfClass(World, ADirectionalLight::StaticClass(), existing_lights); + if (existing_lights.Num() == 0) { + ADirectionalLight* sun = World->SpawnActor( + ADirectionalLight::StaticClass(), FVector::ZeroVector, FRotator(-50.0f, -30.0f, 0.0f), sp); + if (sun) { + sun->GetRootComponent()->SetMobility(EComponentMobility::Movable); + UDirectionalLightComponent* lc = Cast(sun->GetLightComponent()); + if (lc) { + lc->SetIntensity(10.0f); // lux-based + lc->SetLightColor(FLinearColor(1.0f, 0.95f, 0.85f)); + } + spawned_actors_.Add(sun); + } + } + + // --- Sky light (ambient) --- + { + AActor* sla = World->SpawnActor(AActor::StaticClass(), FVector::ZeroVector, FRotator::ZeroRotator, sp); + if (sla) { + USceneComponent* root = NewObject(sla, TEXT("Root")); + sla->SetRootComponent(root); + root->RegisterComponent(); + USkyLightComponent* sl = NewObject(sla, TEXT("SkyLight")); + sl->SetupAttachment(root); + sl->SetIntensity(3.0f); + sl->SetLightColor(FLinearColor(0.5f, 0.6f, 1.0f)); + sl->SourceType = ESkyLightSourceType::SLS_CapturedScene; + sl->bLowerHemisphereIsBlack = false; + sl->RegisterComponent(); + sl->RecaptureSky(); + spawned_actors_.Add(sla); + } + } + + // --- Post-process: sky color + exposure fix --- + { + AActor* ppa = World->SpawnActor(AActor::StaticClass(), FVector::ZeroVector, FRotator::ZeroRotator, sp); + if (ppa) { + USceneComponent* root = NewObject(ppa, TEXT("Root")); + ppa->SetRootComponent(root); + root->RegisterComponent(); + UPostProcessComponent* pp = NewObject(ppa, TEXT("PP")); + pp->SetupAttachment(root); + pp->bUnbound = true; // Affect entire world + pp->Settings.bOverride_AutoExposureMinBrightness = true; + pp->Settings.AutoExposureMinBrightness = 1.0f; + pp->Settings.bOverride_AutoExposureMaxBrightness = true; + pp->Settings.AutoExposureMaxBrightness = 2.0f; + pp->Settings.bOverride_AutoExposureBias = true; + pp->Settings.AutoExposureBias = 1.0f; + pp->RegisterComponent(); + spawned_actors_.Add(ppa); + } + } + + // --- Ground: use a very flat CUBE (visible from all angles) --- + { + AStaticMeshActor* ground = World->SpawnActor( + AStaticMeshActor::StaticClass(), FVector(0, 0, -55), FRotator::ZeroRotator, sp); + if (ground) { + ground->GetStaticMeshComponent()->SetStaticMesh(cube_mesh); + // Cube is 100x100x100, scale to 500m x 500m x 5cm + ground->SetActorScale3D(FVector(500.0f, 500.0f, 0.01f)); + ground->GetStaticMeshComponent()->SetMobility(EComponentMobility::Movable); + if (base_mat) ground->GetStaticMeshComponent()->SetMaterial(0, base_mat); + spawned_actors_.Add(ground); + } + } + + // --- Buildings: tall cubes arranged in a grid --- + { + struct Building + { + FVector pos; + FVector scale; + }; + Building buildings[] = { + // Near-field reference buildings (within 50m) + { { 1500, 1500, 500 }, { 3.0f, 3.0f, 10.0f } }, + { { 1500, -1500, 500 }, { 3.0f, 3.0f, 10.0f } }, + { { -1500, 1500, 500 }, { 3.0f, 3.0f, 10.0f } }, + { { -1500, -1500, 500 }, { 3.0f, 3.0f, 10.0f } }, + // Street-like arrangement + { { 3000, 500, 750 }, { 2.0f, 5.0f, 15.0f } }, + { { 3000, -500, 750 }, { 2.0f, 5.0f, 15.0f } }, + { { 5000, 500, 500 }, { 3.0f, 3.0f, 10.0f } }, + { { 5000, -500, 1000 }, { 2.0f, 2.0f, 20.0f } }, + // Distant landmarks + { { 8000, 0, 1500 }, { 5.0f, 5.0f, 30.0f } }, + { { 0, 8000, 1000 }, { 4.0f, 4.0f, 20.0f } }, + { { -5000, 3000, 600 }, { 6.0f, 3.0f, 12.0f } }, + { { 4000, 4000, 400 }, { 3.0f, 8.0f, 8.0f } }, + }; + for (const auto& b : buildings) { + AStaticMeshActor* bld = World->SpawnActor( + AStaticMeshActor::StaticClass(), b.pos, FRotator::ZeroRotator, sp); + if (bld) { + bld->GetStaticMeshComponent()->SetStaticMesh(cube_mesh); + bld->SetActorScale3D(b.scale); + bld->GetStaticMeshComponent()->SetMobility(EComponentMobility::Movable); + if (base_mat) bld->GetStaticMeshComponent()->SetMaterial(0, base_mat); + spawned_actors_.Add(bld); + } + } + } + + // --- Grid lines on ground for visual reference --- + { + for (int i = -4; i <= 4; i++) { + if (i == 0) continue; // Skip center + // X-direction lines (roads) + AStaticMeshActor* xline = World->SpawnActor( + AStaticMeshActor::StaticClass(), FVector(0, i * 2000, -40), FRotator::ZeroRotator, sp); + if (xline) { + xline->GetStaticMeshComponent()->SetStaticMesh(cube_mesh); + xline->SetActorScale3D(FVector(100.0f, 0.1f, 0.02f)); + xline->GetStaticMeshComponent()->SetMobility(EComponentMobility::Movable); + if (base_mat) xline->GetStaticMeshComponent()->SetMaterial(0, base_mat); + spawned_actors_.Add(xline); + } + // Y-direction lines + AStaticMeshActor* yline = World->SpawnActor( + AStaticMeshActor::StaticClass(), FVector(i * 2000, 0, -40), FRotator::ZeroRotator, sp); + if (yline) { + yline->GetStaticMeshComponent()->SetStaticMesh(cube_mesh); + yline->SetActorScale3D(FVector(0.1f, 100.0f, 0.02f)); + yline->GetStaticMeshComponent()->SetMobility(EComponentMobility::Movable); + if (base_mat) yline->GetStaticMeshComponent()->SetMaterial(0, base_mat); + spawned_actors_.Add(yline); + } + } + } + + UE_LOG(LogTemp, Warning, TEXT("AirSim: Test environment spawned - ground, buildings, grid")); + } + } + initializeTimeOfDay(); AirSimSettings::TimeOfDaySetting tod_setting = getSettings().tod_setting; setTimeOfDay(tod_setting.enabled, tod_setting.start_datetime, tod_setting.is_start_datetime_dst, tod_setting.celestial_clock_speed, tod_setting.update_interval_secs, tod_setting.move_sun); @@ -153,6 +330,9 @@ void ASimModeBase::BeginPlay() } UAirBlueprintLib::GenerateActorMap(this, scene_object_map); + // Note: CARLA Episode creation and initialization is handled by SimWorldGameMode + // (via ACarlaGameModeBase constructor and BeginPlay). + loading_screen_widget_->AddToViewport(); loading_screen_widget_->SetVisibility(ESlateVisibility::Hidden); } diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index 176cb610f..300ef22ba 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -1,5 +1,5 @@ #include "SimModeWorldBase.h" -#include "physics/FastPhysicsEngine.hpp" +#include "physics/FastPhysicsEngine.hpp" // ground lock fix v0.1.2 #include "physics/ExternalPhysicsEngine.hpp" #include #include "AirBlueprintLib.h" diff --git a/Unreal/Plugins/AirSim/Source/SimWorldGameMode.cpp b/Unreal/Plugins/AirSim/Source/SimWorldGameMode.cpp new file mode 100644 index 000000000..1ebd9e592 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/SimWorldGameMode.cpp @@ -0,0 +1,1235 @@ +// SimWorldGameMode.cpp — Unified CARLA + AirSim GameMode +// All methods ported from SimHUD.cpp and AirSimGameMode.cpp with minimal changes. +// v0.1.5: Scroll wheel speed, physics/invincible toggle (P), help overlay (H). + +#include "SimWorldGameMode.h" +#include "UObject/ConstructorHelpers.h" +#include "Kismet/KismetSystemLibrary.h" +#include "Misc/FileHelper.h" +#include "IImageWrapperModule.h" + +#include "Vehicles/Multirotor/SimModeWorldMultiRotor.h" +#include "Vehicles/Multirotor/MultirotorPawnSimApi.h" +#include "Vehicles/Car/SimModeCar.h" +#include "Vehicles/ComputerVision/SimModeComputerVision.h" + +#include "common/AirSimSettings.hpp" +#include "common/Common.hpp" +#include "AirBlueprintLib.h" +#include "Carla/Game/CarlaHUD.h" +#include "Carla/Game/CarlaStatics.h" +#include "Carla/Weather/Weather.h" +#include "Carla/Actor/CarlaActorFactory.h" +#include "Carla/Actor/CarlaActorFactoryBlueprint.h" +#include "Carla/Sensor/SensorFactory.h" +#include "Carla/Actor/StaticMeshFactory.h" +#include "Carla/Trigger/TriggerFactory.h" +#include "Carla/Actor/UtilActorFactory.h" +#include "Carla/AI/AIControllerFactory.h" +#include "Carla/Actor/ActorDispatcher.h" +#include "Carla/Game/CarlaGameInstance.h" + +#include "GameFramework/SpectatorPawn.h" +#include "Kismet/GameplayStatics.h" + +#include "Widgets/SOverlay.h" +#include "Widgets/SBoxPanel.h" +#include "Widgets/Layout/SBorder.h" +#include "Widgets/Layout/SSpacer.h" +#include "Widgets/Text/STextBlock.h" +#include "Styling/CoreStyle.h" + +#include +#include + +// ---------- AirSim logger (separate instance for SimWorldGameMode) ---------- + +class ASimWorldUnrealLog : public msr::airlib::Utils::Logger +{ +public: + virtual void log(int level, const std::string& message) override + { + size_t tab_pos; + static const std::string delim = ":\t"; + if ((tab_pos = message.find(delim)) != std::string::npos) { + UAirBlueprintLib::LogMessageString(message.substr(0, tab_pos), + message.substr(tab_pos + delim.size(), std::string::npos), + LogDebugLevel::Informational); + return; + } + + if (level == msr::airlib::Utils::kLogLevelError) { + UE_LOG(LogTemp, Error, TEXT("%s"), *FString(message.c_str())); + } + else if (level == msr::airlib::Utils::kLogLevelWarn) { + UE_LOG(LogTemp, Warning, TEXT("%s"), *FString(message.c_str())); + } + else { + UE_LOG(LogTemp, Log, TEXT("%s"), *FString(message.c_str())); + } + + msr::airlib::Utils::Logger::log(level, message); + } +}; + +static ASimWorldUnrealLog GlobalSimWorldLog; + +// ========================== FDroneControlWorker ========================== +// Background thread: reads shared velocity/yaw, calls blocking moveByVelocity() + +class FDroneControlWorker : public FRunnable +{ +public: + FDroneControlWorker(ASimModeBase* SimMode, FCriticalSection* Lock, + FVector* VelocityNED, float* Yaw, bool* ShouldHover, bool* DroneReady) + : SimMode_(SimMode), Lock_(Lock), VelocityNED_(VelocityNED), Yaw_(Yaw), bShouldHover_(ShouldHover), bDroneReady_(DroneReady) + { + } + + virtual bool Init() override + { + bRunning_ = true; + return true; + } + + virtual uint32 Run() override + { + using namespace msr::airlib; + + // Get the multirotor API + auto* PawnApi = SimMode_ ? SimMode_->getVehicleSimApi() : nullptr; + if (!PawnApi) { + UE_LOG(LogTemp, Error, TEXT("FDroneControlWorker: No vehicle sim API!")); + return 1; + } + + auto* MultiPawnApi = static_cast(PawnApi); + MultirotorApiBase* DroneApi = MultiPawnApi->getVehicleApi(); + if (!DroneApi) { + UE_LOG(LogTemp, Error, TEXT("FDroneControlWorker: No multirotor API!")); + return 1; + } + + // Don't call enableApiControl/armDisarm at startup — this allows + // external Python API to control the drone freely. FPS control will + // activate on first keyboard input. + UE_LOG(LogTemp, Log, TEXT("FDroneControlWorker: Ready. Use WASD/QE for FPS control, or Python API for scripted flight.")); + + *bDroneReady_ = true; + bool bWasHovering = true; // true = don't hover on startup + bool bFPSActivated = false; // becomes true after first keyboard input + + // Main control loop + while (bRunning_) { + FVector Vel; + float YawCmd; + bool Hover; + + // Read shared state + { + FScopeLock ScopeLock(Lock_); + Vel = *VelocityNED_; + YawCmd = *Yaw_; + Hover = *bShouldHover_; + } + + // If no keyboard input has ever been detected, just sleep (let Python API work) + if (!bFPSActivated && Hover) { + FPlatformProcess::Sleep(0.1f); + continue; + } + + try { + // First keyboard/mouse input: take API control + if (!bFPSActivated && !Hover) { + bFPSActivated = true; + DroneApi->enableApiControl(true); + DroneApi->armDisarm(true); + UE_LOG(LogTemp, Log, TEXT("FDroneControlWorker: FPS control activated!")); + } + + if (bFPSActivated) { + // Always send velocity + yaw (even when hovering) + // This ensures yaw updates even without WASD input + YawMode yaw_mode(false, YawCmd); + DroneApi->moveByVelocity( + Hover ? 0.0f : Vel.X, + Hover ? 0.0f : Vel.Y, + Hover ? 0.0f : Vel.Z, + 0.1f, + DrivetrainType::MaxDegreeOfFreedom, + yaw_mode); + } + else { + // Not yet activated: sleep and let Python API work + FPlatformProcess::Sleep(0.05f); + } + } + catch (const std::exception& ex) { + UE_LOG(LogTemp, Warning, TEXT("FDroneControlWorker: %s"), *FString(ex.what())); + FPlatformProcess::Sleep(0.1f); + } + } + + // Cleanup on shutdown (only if FPS was activated) + if (bFPSActivated) { + UE_LOG(LogTemp, Log, TEXT("FDroneControlWorker: Landing...")); + try { + DroneApi->hover(); + DroneApi->land(10.0f); + DroneApi->armDisarm(false); + DroneApi->enableApiControl(false); + } + catch (const std::exception& ex) { + UE_LOG(LogTemp, Warning, TEXT("FDroneControlWorker: Landing error: %s"), *FString(ex.what())); + } + } + + return 0; + } + + virtual void Stop() override + { + bRunning_ = false; + } + + virtual void Exit() override + { + } + +private: + ASimModeBase* SimMode_ = nullptr; + FCriticalSection* Lock_ = nullptr; + FVector* VelocityNED_ = nullptr; + float* Yaw_ = nullptr; + bool* bShouldHover_ = nullptr; + bool* bDroneReady_ = nullptr; + volatile bool bRunning_ = false; +}; + +// ========================== Constructor ========================== + +ASimWorldGameMode::ASimWorldGameMode(const FObjectInitializer& ObjectInitializer) + : Super(ObjectInitializer) // CARLA creates Episode, Recorder, TaggerDelegate, etc. +{ + // AirSim manages its own vehicles; CARLA spawns via ActorFactories. + // Setting DefaultPawnClass = nullptr prevents UE4 from auto-spawning a pawn + // that would break AirSim drone control. + // We manually create a spectator pawn in BeginPlay() for CARLA compatibility. + DefaultPawnClass = nullptr; + + // Keep HUDClass = ACarlaHUD from parent (AirSim UI uses UMG widget instead) + // HUDClass is already set to ACarlaHUD::StaticClass() by ACarlaGameModeBase constructor. + + // --- CARLA Blueprint properties (normally set in CarlaGameMode blueprint) --- + + // Weather blueprint class + static ConstructorHelpers::FClassFinder weather_class( + TEXT("/Game/Carla/Blueprints/Weather/BP_Weather")); + if (weather_class.Succeeded()) { + WeatherClass = weather_class.Class; + } + + // Actor Factories — C++ factories + ActorFactories.Add(ASensorFactory::StaticClass()); + ActorFactories.Add(AStaticMeshFactory::StaticClass()); + ActorFactories.Add(ATriggerFactory::StaticClass()); + ActorFactories.Add(AUtilActorFactory::StaticClass()); + ActorFactories.Add(AAIControllerFactory::StaticClass()); + + // Actor Factories — Blueprint factories (vehicles, walkers, props) + static ConstructorHelpers::FClassFinder vehicle_factory( + TEXT("/Game/Carla/Blueprints/Vehicles/VehicleFactory")); + if (vehicle_factory.Succeeded()) + ActorFactories.Add(vehicle_factory.Class); + + static ConstructorHelpers::FClassFinder walker_factory( + TEXT("/Game/Carla/Blueprints/Walkers/WalkerFactory")); + if (walker_factory.Succeeded()) + ActorFactories.Add(walker_factory.Class); + + static ConstructorHelpers::FClassFinder prop_factory( + TEXT("/Game/Carla/Blueprints/Props/PropFactory")); + if (prop_factory.Succeeded()) + ActorFactories.Add(prop_factory.Class); + + // --- AirSim setup --- + + // Initialize AirSim logger + common_utils::Utils::getSetLogger(&GlobalSimWorldLog); + + // Pre-load ImageWrapper module (must be done on main thread) + static IImageWrapperModule& ImageWrapperModule = + FModuleManager::LoadModuleChecked(TEXT("ImageWrapper")); + + // Load AirSim HUD widget blueprint + static ConstructorHelpers::FClassFinder hud_widget_class( + TEXT("WidgetBlueprint'/AirSim/Blueprints/BP_SimHUDWidget'")); + WidgetClass_ = hud_widget_class.Succeeded() ? hud_widget_class.Class : nullptr; +} + +// ========================== BeginPlay ========================== + +void ASimWorldGameMode::BeginPlay() +{ + // Full CARLA initialization: Episode, Weather, Traffic, Recorder, etc. + Super::BeginPlay(); + + // Manually create a spectator pawn for CARLA API compatibility. + // DefaultPawnClass is nullptr (required for AirSim drone control), so + // Episode->InitializeAtBeginPlay() could not find a spectator. + // We create one now and register it with CARLA's Episode. + // IMPORTANT: Do NOT possess the pawn — possessing it breaks AirSim's input handling. + { + auto* World = GetWorld(); + if (World) { + FActorSpawnParameters SpawnParams; + SpawnParams.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AlwaysSpawn; + auto* SpectatorPawn = World->SpawnActor( + ASpectatorPawn::StaticClass(), FTransform::Identity, SpawnParams); + if (SpectatorPawn) { + CachedSpectator_ = SpectatorPawn; + + // Register with CARLA Episode (get via GameInstance for non-const access) + auto* GI = Cast(GetGameInstance()); + auto* Episode = GI ? GI->GetCarlaEpisode() : nullptr; + if (Episode) { + Episode->Spectator = SpectatorPawn; + FActorDescription Description; + Description.Id = TEXT("spectator"); + Description.Class = SpectatorPawn->GetClass(); + Episode->ActorDispatcher->RegisterActor(*SpectatorPawn, Description); + UE_LOG(LogTemp, Log, TEXT("SimWorldGameMode: Spectator pawn created and registered.")); + } + } + } + } + + UE_LOG(LogTemp, Log, TEXT("SimWorldGameMode: CARLA BeginPlay complete, starting AirSim bootstrap...")); + + try { + UAirBlueprintLib::OnBeginPlay(); + + // NO loadLevel() — map already loaded by CARLA/UE4. AirSim's level_name setting is ignored. + + InitializeAirSimSettings(); + SetUnrealEngineSettings(); + CreateSimMode(); + CreateAirSimWidget(); + SetupAirSimInputBindings(); + + if (SimMode_) + SimMode_->startApiServer(); + + UE_LOG(LogTemp, Log, TEXT("SimWorldGameMode: AirSim bootstrap complete. API server started.")); + + // Setup FPS drone control (must be after SimMode creation) + SetupFPSControl(); + } + catch (std::exception& ex) { + UAirBlueprintLib::LogMessageString("Error at AirSim startup: ", ex.what(), LogDebugLevel::Failure); + UAirBlueprintLib::ShowMessage(EAppMsgType::Ok, + std::string("Error at AirSim startup: ") + ex.what(), + "Error"); + } +} + +// ========================== Tick ========================== + +void ASimWorldGameMode::Tick(float DeltaSeconds) +{ + // CARLA: Recorder tick + Super::Tick(DeltaSeconds); + + // One-time: register AirSim drone pawn with CARLA ActorDispatcher + // so Python scripts can find it via world.get_actors() + if (!bDroneRegistered_ && SimMode_) { + auto* PawnApi = SimMode_->getVehicleSimApi(); + if (PawnApi) { + auto* DroneSimApi = static_cast(PawnApi); + APawn* DronePawn = DroneSimApi->getPawn(); + if (DronePawn) { + auto* GI = Cast(GetGameInstance()); + auto* Episode = GI ? GI->GetCarlaEpisode() : nullptr; + if (Episode) { + FActorDescription Description; + Description.Id = TEXT("airsim.drone"); + Description.Class = DronePawn->GetClass(); + Episode->ActorDispatcher->RegisterActor(*DronePawn, Description); + UE_LOG(LogTemp, Log, TEXT("SimWorldGameMode: Drone pawn registered with CARLA (type_id=airsim.drone)")); + } + bDroneRegistered_ = true; + } + } + } + + // AirSim: Update debug report widget + if (SimMode_ && SimMode_->EnableReport && Widget_) + Widget_->updateDebugReport(SimMode_->getDebugReport()); + + // FPS drone control: read input, update shared state + if (bFPSControlActive_) { + UpdateFPSControl(DeltaSeconds); + UpdateCameraFollow(); + if (bShowHelp_) + DrawHelpOverlay(); + } +} + +// ========================== EndPlay ========================== + +void ASimWorldGameMode::EndPlay(const EEndPlayReason::Type EndPlayReason) +{ + // Stop drone control thread + if (DroneWorker_) { + DroneWorker_->Stop(); + } + if (DroneThread_) { + DroneThread_->WaitForCompletion(); + delete DroneThread_; + DroneThread_ = nullptr; + } + if (DroneWorker_) { + delete DroneWorker_; + DroneWorker_ = nullptr; + } + bFPSControlActive_ = false; + bDroneReady_ = false; + + // Stop AirSim API server + if (SimMode_) + SimMode_->stopApiServer(); + + // Remove help overlay + if (HelpOverlayWrapper_.IsValid() && GEngine && GEngine->GameViewport) { + GEngine->GameViewport->RemoveViewportWidgetContent(HelpOverlayWrapper_.ToSharedRef()); + } + HelpOverlayWrapper_.Reset(); + HelpTitleBlock_.Reset(); + HelpSubtitleBlock_.Reset(); + HelpContentBlock_.Reset(); + HelpStatusBlock_.Reset(); + HelpOverlayContainer_.Reset(); + + // Destroy AirSim widget + if (Widget_) { + Widget_->Destruct(); + Widget_ = nullptr; + } + + // Destroy SimMode actor + if (SimMode_) { + SimMode_->Destroy(); + SimMode_ = nullptr; + } + + UAirBlueprintLib::OnEndPlay(); + + // CARLA cleanup: Episode->EndPlay(), GameInstance->NotifyEndEpisode(), etc. + Super::EndPlay(EndPlayReason); +} + +// ========================== FPS Drone Control ========================== + +void ASimWorldGameMode::SetupFPSControl() +{ + if (!SimMode_) { + UE_LOG(LogTemp, Warning, TEXT("SetupFPSControl: No SimMode, skipping FPS control.")); + return; + } + + // Only enable for multirotor mode + std::string simmode_name = AirSimSettings::singleton().simmode_name; + if (simmode_name != AirSimSettings::kSimModeTypeMultirotor) { + UE_LOG(LogTemp, Log, TEXT("SetupFPSControl: SimMode is not Multirotor, skipping FPS control.")); + return; + } + + APlayerController* PC = GetWorld()->GetFirstPlayerController(); + if (!PC) { + UE_LOG(LogTemp, Warning, TEXT("SetupFPSControl: No PlayerController!")); + return; + } + + // Set mouse capture for FPS control + PC->bShowMouseCursor = false; + FInputModeGameOnly InputMode; + PC->SetInputMode(InputMode); + + // Force viewport mouse lock — required since DefaultPawnClass=nullptr means + // no possessed pawn, and UE4's default mouse capture depends on pawn possession + if (UGameViewportClient* ViewportClient = GetWorld()->GetGameViewport()) { + ViewportClient->SetMouseLockMode(EMouseLockMode::LockAlways); + ViewportClient->SetCaptureMouseOnClick(EMouseCaptureMode::CapturePermanently); + } + + // Set view target to spectator + if (CachedSpectator_) + PC->SetViewTarget(CachedSpectator_); + + // Build weather presets + { + // ClearNoon + FWeatherParameters w; + w.Cloudiness = 10.0f; + w.SunAltitudeAngle = 75.0f; + w.SunAzimuthAngle = 0.0f; + WeatherPresets_.Add(w); + } + { + // Cloudy + FWeatherParameters w; + w.Cloudiness = 80.0f; + w.SunAltitudeAngle = 50.0f; + WeatherPresets_.Add(w); + } + { + // Rain + FWeatherParameters w; + w.Cloudiness = 90.0f; + w.Precipitation = 80.0f; + w.PrecipitationDeposits = 60.0f; + w.Wetness = 80.0f; + w.SunAltitudeAngle = 40.0f; + WeatherPresets_.Add(w); + } + { + // Sunset + FWeatherParameters w; + w.Cloudiness = 30.0f; + w.SunAltitudeAngle = 5.0f; + w.SunAzimuthAngle = 270.0f; + WeatherPresets_.Add(w); + } + { + // Night + FWeatherParameters w; + w.Cloudiness = 20.0f; + w.SunAltitudeAngle = -80.0f; + WeatherPresets_.Add(w); + } + { + // DustStorm + FWeatherParameters w; + w.Cloudiness = 60.0f; + w.DustStorm = 80.0f; + w.FogDensity = 30.0f; + w.WindIntensity = 80.0f; + w.SunAltitudeAngle = 40.0f; + WeatherPresets_.Add(w); + } + + // Bind FPS control keys (true = fire on key press, not release) + UAirBlueprintLib::BindActionToKey("InputEventNextWeather", EKeys::N, this, &ASimWorldGameMode::InputEventNextWeather, true); + UAirBlueprintLib::BindActionToKey("InputEventToggleMouseCapture", EKeys::Tab, this, &ASimWorldGameMode::InputEventToggleMouseCapture, true); + UAirBlueprintLib::BindActionToKey("InputEventSpeedUp", EKeys::MouseScrollUp, this, &ASimWorldGameMode::InputEventSpeedUp, true); + UAirBlueprintLib::BindActionToKey("InputEventSpeedDown", EKeys::MouseScrollDown, this, &ASimWorldGameMode::InputEventSpeedDown, true); + UAirBlueprintLib::BindActionToKey("InputEventToggleHelpOverlay", EKeys::H, this, &ASimWorldGameMode::InputEventToggleHelpOverlay, true); + UAirBlueprintLib::BindActionToKey("InputEventTogglePhysicsMode", EKeys::P, this, &ASimWorldGameMode::InputEventTogglePhysicsMode, true); + + // Start drone control thread + DroneWorker_ = new FDroneControlWorker( + SimMode_, &DroneControlLock_, &DesiredVelocityNED_, &DesiredYaw_, &bShouldHover_, &bDroneReady_); + DroneThread_ = FRunnableThread::Create(DroneWorker_, TEXT("DroneControlThread")); + + bFPSControlActive_ = true; + UE_LOG(LogTemp, Log, TEXT("SetupFPSControl: FPS drone control active. WASD=Move, Mouse=Look, Scroll=Speed, N=Weather, H=Help, P=PhysicsToggle")); +} + +void ASimWorldGameMode::UpdateFPSControl(float DeltaSeconds) +{ + APlayerController* PC = GetWorld()->GetFirstPlayerController(); + if (!PC || !bDroneReady_) + return; + + // v0.1.3: Initialize FPSYaw_ from drone's actual orientation (once) + // Fixes W key flying in wrong direction on first press + if (!bYawInitialized_) { + auto* PawnApi = SimMode_ ? SimMode_->getVehicleSimApi() : nullptr; + if (PawnApi) { + FRotator DroneRot = PawnApi->getUUOrientation(); + FPSYaw_ = DroneRot.Yaw; + bYawInitialized_ = true; + } + } + + // ---- Mouse look: yaw only ---- + if (bMouseCaptured_) { + float MouseX = 0.0f, MouseY = 0.0f; + PC->GetInputMouseDelta(MouseX, MouseY); + + FPSYaw_ += MouseX * 0.35f; + + while (FPSYaw_ >= 360.0f) + FPSYaw_ -= 360.0f; + while (FPSYaw_ < 0.0f) + FPSYaw_ += 360.0f; + } + + // ---- Keyboard movement: horizontal + vertical ---- + // WASD = move on horizontal plane (yaw-relative), Space/Shift = up/down + float Fwd = 0.0f, Strafe = 0.0f, Vert = 0.0f; + if (PC->IsInputKeyDown(EKeys::W)) Fwd += 1.0f; + if (PC->IsInputKeyDown(EKeys::S)) Fwd -= 1.0f; + if (PC->IsInputKeyDown(EKeys::D)) Strafe += 1.0f; + if (PC->IsInputKeyDown(EKeys::A)) Strafe -= 1.0f; + if (PC->IsInputKeyDown(EKeys::SpaceBar)) Vert += 1.0f; // ascend + if (PC->IsInputKeyDown(EKeys::LeftShift)) Vert -= 1.0f; // descend + + bool bHasInput = (FMath::Abs(Fwd) > 0.01f || FMath::Abs(Strafe) > 0.01f || FMath::Abs(Vert) > 0.01f); + + // Horizontal velocity decomposed by yaw only (no pitch) + float YawRad = FMath::DegreesToRadians(FPSYaw_); + float VxNED = (Fwd * FMath::Cos(YawRad) - Strafe * FMath::Sin(YawRad)) * DroneSpeed_; + float VyNED = (Fwd * FMath::Sin(YawRad) + Strafe * FMath::Cos(YawRad)) * DroneSpeed_; + float VzNED = -Vert * DroneSpeed_; // NED: negative Z = up + + // Write to shared state + { + FScopeLock ScopeLock(&DroneControlLock_); + DesiredVelocityNED_ = FVector(VxNED, VyNED, VzNED); + DesiredYaw_ = FPSYaw_; + bShouldHover_ = !bHasInput; + } +} + +void ASimWorldGameMode::UpdateCameraFollow() +{ + if (!SimMode_ || !CachedSpectator_ || !bDroneReady_) + return; + + auto* PawnApi = SimMode_->getVehicleSimApi(); + if (!PawnApi) + return; + + FVector DronePos = PawnApi->getUUPosition(); + FRotator DroneRot = PawnApi->getUUOrientation(); // drone's actual UE4 rotation + + // v0.1.3: Always 3rd person (removed 1st person toggle) + // 3rd person: fixed chase camera behind drone + FRotator YawOnly(0.0f, DroneRot.Yaw, 0.0f); + FVector Behind = YawOnly.Vector() * -800.0f; // 8m behind + Behind.Z += 350.0f; // 3.5m above + + FVector CamPos = DronePos + Behind; + FRotator LookAt = (DronePos - CamPos).Rotation(); + + CachedSpectator_->SetActorLocation(CamPos); + CachedSpectator_->SetActorRotation(LookAt); +} + +// ========================== FPS Input Handlers ========================== + +void ASimWorldGameMode::InputEventNextWeather() +{ + if (WeatherPresets_.Num() == 0) return; + + WeatherIndex_ = (WeatherIndex_ + 1) % WeatherPresets_.Num(); + const FWeatherParameters& Preset = WeatherPresets_[WeatherIndex_]; + + auto* Episode = UCarlaStatics::GetCurrentEpisode(GetWorld()); + if (Episode) { + AWeather* Weather = Episode->GetWeather(); + if (Weather) { + Weather->ApplyWeather(Preset); + } + } + + static const TCHAR* PresetNames[] = { + TEXT("ClearNoon"), TEXT("Cloudy"), TEXT("Rain"), TEXT("Sunset"), TEXT("Night"), TEXT("DustStorm") + }; + UE_LOG(LogTemp, Log, TEXT("Weather: %s"), PresetNames[WeatherIndex_]); +} + +void ASimWorldGameMode::InputEventToggleMouseCapture() +{ + APlayerController* PC = GetWorld()->GetFirstPlayerController(); + if (!PC) return; + + bMouseCaptured_ = !bMouseCaptured_; + UGameViewportClient* ViewportClient = GetWorld()->GetGameViewport(); + + if (bMouseCaptured_) { + PC->bShowMouseCursor = false; + FInputModeGameOnly InputMode; + PC->SetInputMode(InputMode); + if (ViewportClient) { + ViewportClient->SetMouseLockMode(EMouseLockMode::LockAlways); + ViewportClient->SetCaptureMouseOnClick(EMouseCaptureMode::CapturePermanently); + } + } + else { + PC->bShowMouseCursor = true; + FInputModeGameAndUI InputMode; + InputMode.SetHideCursorDuringCapture(false); + PC->SetInputMode(InputMode); + if (ViewportClient) { + ViewportClient->SetMouseLockMode(EMouseLockMode::DoNotLock); + ViewportClient->SetCaptureMouseOnClick(EMouseCaptureMode::NoCapture); + } + } + UE_LOG(LogTemp, Log, TEXT("Mouse capture: %s"), bMouseCaptured_ ? TEXT("ON") : TEXT("OFF")); +} + +void ASimWorldGameMode::InputEventSpeedUp() +{ + DroneSpeed_ = FMath::Min(30.0f, DroneSpeed_ + 1.0f); + UE_LOG(LogTemp, Log, TEXT("Drone speed: %.0f m/s"), DroneSpeed_); +} + +void ASimWorldGameMode::InputEventSpeedDown() +{ + DroneSpeed_ = FMath::Max(1.0f, DroneSpeed_ - 1.0f); + UE_LOG(LogTemp, Log, TEXT("Drone speed: %.0f m/s"), DroneSpeed_); +} + +void ASimWorldGameMode::InputEventToggleHelpOverlay() +{ + bShowHelp_ = !bShowHelp_; + ShowHelpOverlay(bShowHelp_); + UE_LOG(LogTemp, Log, TEXT("Help overlay: %s"), bShowHelp_ ? TEXT("ON") : TEXT("OFF")); +} + +void ASimWorldGameMode::InputEventTogglePhysicsMode() +{ + bPhysicsCollision_ = !bPhysicsCollision_; + + // Push to MultirotorPawnSimApi + if (SimMode_) { + auto* PawnApi = SimMode_->getVehicleSimApi(); + if (PawnApi) { + auto* MultiPawnApi = static_cast(PawnApi); + MultiPawnApi->setPhysicsCollisionEnabled(bPhysicsCollision_); + } + } + + // On-screen notification (2s) + if (GEngine) { + FString Msg = bPhysicsCollision_ + ? TEXT("Physics Mode ON / 物理碰撞模式") + : TEXT("Invincible Mode ON / 无敌穿越模式"); + GEngine->AddOnScreenDebugMessage(7777, 2.0f, FColor::Yellow, Msg); + } + UE_LOG(LogTemp, Log, TEXT("Collision mode: %s"), bPhysicsCollision_ ? TEXT("Physics") : TEXT("Invincible")); +} + +void ASimWorldGameMode::DrawHelpOverlay() +{ + // Now handled by Slate widget — just update dynamic text + if (bShowHelp_ && HelpContentBlock_.IsValid()) { + UpdateHelpOverlayText(); + } +} + +void ASimWorldGameMode::CreateHelpOverlayWidget() +{ + if (HelpOverlayWrapper_.IsValid()) + return; + + // Font setup — DroidSansFallback for CJK support + FString FontPath = FPaths::EngineContentDir() / TEXT("Slate/Fonts/DroidSansFallback.ttf"); + FSlateFontInfo TitleFont(FontPath, 30); + TitleFont.TypefaceFontName = FName("Bold"); + FSlateFontInfo SubtitleFont(FontPath, 16); + SubtitleFont.TypefaceFontName = FName("Bold"); + FSlateFontInfo ContentFont(FontPath, 20); + ContentFont.TypefaceFontName = FName("Bold"); + FSlateFontInfo StatusFont(FontPath, 18); + StatusFont.TypefaceFontName = FName("Bold"); + FSlateFontInfo KeyFont(FontPath, 18); + KeyFont.TypefaceFontName = FName("Bold"); + + // Color palette — high contrast, solid dark background + FLinearColor TitleColor(1.0f, 1.0f, 1.0f, 1.0f); + FLinearColor SubtitleColor(0.8f, 0.8f, 0.85f, 1.0f); + FLinearColor ContentColor(1.0f, 1.0f, 1.0f, 1.0f); + FLinearColor AccentColor(0.38f, 0.68f, 1.0f, 1.0f); // SF Blue + FLinearColor KeyColor(1.0f, 1.0f, 1.0f, 1.0f); + FLinearColor SeparatorColor(1.0f, 1.0f, 1.0f, 0.15f); + FLinearColor BgColor(0.0f, 0.0f, 0.0f, 0.95f); // near-opaque black + + // Title + HelpTitleBlock_ = SNew(STextBlock) + .Font(TitleFont) + .ColorAndOpacity(FSlateColor(TitleColor)) + .Justification(ETextJustify::Left); + + // Subtitle + HelpSubtitleBlock_ = SNew(STextBlock) + .Font(SubtitleFont) + .ColorAndOpacity(FSlateColor(SubtitleColor)) + .Justification(ETextJustify::Left); + + // Main content — with shadow for better readability + HelpContentBlock_ = SNew(STextBlock) + .Font(ContentFont) + .ColorAndOpacity(FSlateColor(ContentColor)) + .ShadowOffset(FVector2D(1.5f, 1.5f)) + .ShadowColorAndOpacity(FLinearColor(0.0f, 0.0f, 0.0f, 0.6f)) + .Justification(ETextJustify::Left); + + // Status bar — same white color as content, no blue + HelpStatusBlock_ = SNew(STextBlock) + .Font(StatusFont) + .ColorAndOpacity(FSlateColor(ContentColor)) + .ShadowOffset(FVector2D(1.5f, 1.5f)) + .ShadowColorAndOpacity(FLinearColor(0.0f, 0.0f, 0.0f, 0.6f)) + .Justification(ETextJustify::Left); + + // Thin separator line (horizontal rule) + auto MakeSeparator = [&SeparatorColor]() -> TSharedRef { + return SNew(SBorder) + .BorderBackgroundColor(SeparatorColor) + .Padding(0) + [SNew(SSpacer) + .Size(FVector2D(1.0f, 1.0f))]; + }; + + // Build layout — compact vertical stack, fits within 1080p viewport + HelpOverlayContainer_ = SNew(SVerticalBox) + // Title + + SVerticalBox::Slot().AutoHeight().Padding(0, 0, 0, 2) + [HelpTitleBlock_.ToSharedRef()] + // Subtitle + + SVerticalBox::Slot().AutoHeight().Padding(0, 0, 0, 10) + [HelpSubtitleBlock_.ToSharedRef()] + // Separator + + SVerticalBox::Slot().AutoHeight().Padding(0, 0, 0, 10) + [MakeSeparator()] + // Content + + SVerticalBox::Slot().AutoHeight().Padding(0, 0, 0, 10) + [HelpContentBlock_.ToSharedRef()] + // Separator + + SVerticalBox::Slot().AutoHeight().Padding(0, 0, 0, 8) + [MakeSeparator()] + // Status + + SVerticalBox::Slot().AutoHeight() + [HelpStatusBlock_.ToSharedRef()]; + + // Wrap in panel — SBox constrains max height to prevent viewport overflow + HelpOverlayWrapper_ = + SNew(SOverlay) + SOverlay::Slot() + .HAlign(HAlign_Left) + .VAlign(VAlign_Top) + .Padding(FMargin(36.0f, 36.0f, 0.0f, 0.0f)) + [SNew(SBox) + .MaxDesiredHeight(980.0f) + .Clipping(EWidgetClipping::ClipToBounds) + [SNew(SBorder) + .BorderBackgroundColor(BgColor) + .Padding(FMargin(28.0f, 22.0f, 36.0f, 20.0f)) + [HelpOverlayContainer_.ToSharedRef()]]]; + + UpdateHelpOverlayText(); +} + +void ASimWorldGameMode::UpdateHelpOverlayText() +{ + if (!HelpContentBlock_.IsValid()) + return; + + // Title + if (HelpTitleBlock_.IsValid()) + HelpTitleBlock_->SetText(FText::FromString(TEXT("CarlaAir"))); + + // Subtitle — v0.1.7 + if (HelpSubtitleBlock_.IsValid()) + HelpSubtitleBlock_->SetText(FText::FromString( + TEXT("v0.1.7 Air-ground integrated simulation platform"))); + // Chinese: 空地一体联合仿真平台 + + // Content — detailed bilingual help + FString Content = FString::Printf(TEXT( + "FLIGHT CONTROLS\n" + "\n" + " W / S Forward / Backward\n" + " A / D Strafe Left / Right\n" + " Mouse Yaw Turn Direction\n" + " Space Ascend Drone\n" + " Left Shift Descend Drone\n" + " Scroll Wheel Adjust flight speed (+/- 1 m/s)\n" + "\n" + "SYSTEM FUNCTIONS\n" + "\n" + " N Cycle Weather Presets\n" + " P Physics / Noclip\n" + " Tab Release / Capture Mouse\n" + " H Show / Hide Help\n" + " 1 / 2 / 3 Sensor Camera Views\n" + "\n" + "ADVANCED (for AirSim experts)\n" + "\n" + " I Toggle First-Person / Default View\n" + " B FPV Mode (mouse controls drone yaw)\n" + " FPV Mode (mouse controls drone yaw)\n" + " I / B For experienced AirSim users only")); + // Chinese decoded: 飞行控制, 前进/后退, 左移/右移, 偏航旋转方向, 上升无人机, 下降无人机, + // 调节飞行速度, 系统功能, 切换天气预设, 物理碰撞/穿越模式, 释放/捕获鼠标, 显示/隐藏帮助, + // 传感器画面, AirSim高级, 切换第一人称/默认视角, FPV模式(鼠标控制无人机偏航), + // 仅建议熟悉AirSim的用户使用 + HelpContentBlock_->SetText(FText::FromString(Content)); + + // Status line — white, same style as content + FString ModeStr = bPhysicsCollision_ + ? TEXT("Physics") + : TEXT("Noclip"); + + if (HelpStatusBlock_.IsValid()) { + FString Status = FString::Printf(TEXT( + "Speed: %.0f m/s | %s | Press H to close"), + DroneSpeed_, + *ModeStr); + // Chinese: 当前速度, 按H关闭 + HelpStatusBlock_->SetText(FText::FromString(Status)); + } +} + +void ASimWorldGameMode::ShowHelpOverlay(bool bShow) +{ + if (!GEngine || !GEngine->GameViewport) + return; + + if (bShow) { + CreateHelpOverlayWidget(); + if (HelpOverlayWrapper_.IsValid()) { + GEngine->GameViewport->AddViewportWidgetContent( + HelpOverlayWrapper_.ToSharedRef(), + 100 // z-order (above most UI) + ); + UpdateHelpOverlayText(); + } + } + else { + if (HelpOverlayWrapper_.IsValid()) { + GEngine->GameViewport->RemoveViewportWidgetContent( + HelpOverlayWrapper_.ToSharedRef()); + } + } +} + +// ========================== AirSim Settings ========================== + +void ASimWorldGameMode::InitializeAirSimSettings() +{ + std::string settingsText; + if (GetSettingsText(settingsText)) + AirSimSettings::initializeSettings(settingsText); + else + AirSimSettings::createDefaultSettingsFile(); + + AirSimSettings::singleton().load(std::bind(&ASimWorldGameMode::GetSimModeFromUser, this)); + + for (const auto& warning : AirSimSettings::singleton().warning_messages) { + UAirBlueprintLib::LogMessageString(warning, "", LogDebugLevel::Failure); + } + for (const auto& error : AirSimSettings::singleton().error_messages) { + UAirBlueprintLib::ShowMessage(EAppMsgType::Ok, error, "settings.json"); + } +} + +void ASimWorldGameMode::SetUnrealEngineSettings() +{ + // Disable motion blur for clean capture images + GetWorld()->GetGameViewport()->GetEngineShowFlags()->SetMotionBlur(false); + + // Enable custom stencil (required for AirSim segmentation) + static const auto custom_depth_var = IConsoleManager::Get().FindConsoleVariable(TEXT("r.CustomDepth")); + custom_depth_var->Set(3); + UKismetSystemLibrary::ExecuteConsoleCommand(GetWorld(), FString("r.CustomDepth 3")); + + // Increase render fence timeout for large environments with stencil IDs + static const auto render_timeout_var = IConsoleManager::Get().FindConsoleVariable(TEXT("g.TimeoutForBlockOnRenderFence")); + render_timeout_var->Set(300000); +} + +// ========================== SimMode Creation ========================== + +void ASimWorldGameMode::CreateSimMode() +{ + std::string simmode_name = AirSimSettings::singleton().simmode_name; + + FActorSpawnParameters simmode_spawn_params; + simmode_spawn_params.SpawnCollisionHandlingOverride = + ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + + // Spawn at origin — used for global NED transforms + if (simmode_name == AirSimSettings::kSimModeTypeMultirotor) + SimMode_ = GetWorld()->SpawnActor( + FVector::ZeroVector, FRotator::ZeroRotator, simmode_spawn_params); + else if (simmode_name == AirSimSettings::kSimModeTypeCar) + SimMode_ = GetWorld()->SpawnActor( + FVector::ZeroVector, FRotator::ZeroRotator, simmode_spawn_params); + else if (simmode_name == AirSimSettings::kSimModeTypeComputerVision) + SimMode_ = GetWorld()->SpawnActor( + FVector::ZeroVector, FRotator::ZeroRotator, simmode_spawn_params); + else { + UAirBlueprintLib::ShowMessage(EAppMsgType::Ok, + std::string("SimMode is not valid: ") + simmode_name, + "Error"); + UAirBlueprintLib::LogMessageString("SimMode is not valid: ", simmode_name, LogDebugLevel::Failure); + } +} + +// ========================== Widget / HUD ========================== + +void ASimWorldGameMode::CreateAirSimWidget() +{ + if (WidgetClass_ != nullptr) { + APlayerController* player_controller = GetWorld()->GetFirstPlayerController(); + auto* pawn = player_controller->GetPawn(); + if (pawn) { + std::string pawn_name = std::string(TCHAR_TO_ANSI(*pawn->GetName())); + Utils::log(pawn_name); + } + else { + UAirBlueprintLib::ShowMessage(EAppMsgType::Ok, + std::string("There were no compatible vehicles created for current SimMode! Check your settings.json."), + "Error"); + UAirBlueprintLib::LogMessage( + TEXT("There were no compatible vehicles created for current SimMode! Check your settings.json."), + TEXT(""), + LogDebugLevel::Failure); + } + + Widget_ = CreateWidget(player_controller, WidgetClass_); + } + else { + Widget_ = nullptr; + UAirBlueprintLib::LogMessage( + TEXT("Cannot instantiate BP_SimHUDWidget blueprint!"), TEXT(""), LogDebugLevel::Failure); + } + + InitializeSubWindows(); + + if (Widget_) { + Widget_->AddToViewport(); + Widget_->initializeForPlay(); + if (SimMode_) + Widget_->setReportVisible(SimMode_->EnableReport); + Widget_->setOnToggleRecordingHandler(std::bind(&ASimWorldGameMode::ToggleRecordHandler, this)); + Widget_->setRecordButtonVisibility(AirSimSettings::singleton().is_record_ui_visible); + UpdateWidgetSubwindowVisibility(); + } +} + +void ASimWorldGameMode::InitializeSubWindows() +{ + if (!SimMode_) + return; + + auto default_vehicle_sim_api = SimMode_->getVehicleSimApi(); + + if (default_vehicle_sim_api) { + auto camera_count = default_vehicle_sim_api->getCameraCount(); + + if (camera_count > 0) { + SubwindowCameras_[0] = default_vehicle_sim_api->getCamera(""); + SubwindowCameras_[1] = default_vehicle_sim_api->getCamera(""); + SubwindowCameras_[2] = default_vehicle_sim_api->getCamera(""); + } + else + SubwindowCameras_[0] = SubwindowCameras_[1] = SubwindowCameras_[2] = nullptr; + } + + for (const auto& setting : GetSubWindowSettings()) { + APIPCamera* camera = SimMode_->getCamera( + msr::airlib::CameraDetails(setting.camera_name, setting.vehicle_name, setting.external)); + if (camera) + SubwindowCameras_[setting.window_index] = camera; + else + UAirBlueprintLib::LogMessageString("Invalid Camera settings in element", + std::to_string(setting.window_index), + LogDebugLevel::Failure); + } +} + +// ========================== Input Bindings ========================== + +void ASimWorldGameMode::SetupAirSimInputBindings() +{ + UAirBlueprintLib::EnableInput(this); + + UAirBlueprintLib::BindActionToKey("inputEventToggleRecording", EKeys::R, this, &ASimWorldGameMode::InputEventToggleRecording); + UAirBlueprintLib::BindActionToKey("InputEventToggleReport", EKeys::Semicolon, this, &ASimWorldGameMode::InputEventToggleReport); + UAirBlueprintLib::BindActionToKey("InputEventToggleHelp", EKeys::F1, this, &ASimWorldGameMode::InputEventToggleHelp); + UAirBlueprintLib::BindActionToKey("InputEventToggleTrace", EKeys::T, this, &ASimWorldGameMode::InputEventToggleTrace); + + UAirBlueprintLib::BindActionToKey("InputEventToggleSubwindow0", EKeys::One, this, &ASimWorldGameMode::InputEventToggleSubwindow0); + UAirBlueprintLib::BindActionToKey("InputEventToggleSubwindow1", EKeys::Two, this, &ASimWorldGameMode::InputEventToggleSubwindow1); + UAirBlueprintLib::BindActionToKey("InputEventToggleSubwindow2", EKeys::Three, this, &ASimWorldGameMode::InputEventToggleSubwindow2); + UAirBlueprintLib::BindActionToKey("InputEventToggleAll", EKeys::Zero, this, &ASimWorldGameMode::InputEventToggleAll); +} + +// ========================== Input Handlers ========================== + +void ASimWorldGameMode::ToggleRecordHandler() +{ + if (SimMode_) + SimMode_->toggleRecording(); +} + +void ASimWorldGameMode::InputEventToggleRecording() +{ + ToggleRecordHandler(); +} + +void ASimWorldGameMode::InputEventToggleReport() +{ + if (SimMode_ && Widget_) { + SimMode_->EnableReport = !SimMode_->EnableReport; + Widget_->setReportVisible(SimMode_->EnableReport); + } +} + +void ASimWorldGameMode::InputEventToggleHelp() +{ + if (Widget_) + Widget_->toggleHelpVisibility(); +} + +void ASimWorldGameMode::InputEventToggleTrace() +{ + if (SimMode_) + SimMode_->toggleTraceAll(); +} + +void ASimWorldGameMode::UpdateWidgetSubwindowVisibility() +{ + if (!Widget_) + return; + + for (int window_index = 0; window_index < AirSimSettings::kSubwindowCount; ++window_index) { + APIPCamera* camera = SubwindowCameras_[window_index]; + ImageType camera_type = GetSubWindowSettings().at(window_index).image_type; + + bool is_visible = GetSubWindowSettings().at(window_index).visible && camera != nullptr; + + if (camera != nullptr) { + camera->setCameraTypeEnabled(camera_type, is_visible); + camera->setCameraTypeUpdate(camera_type, false); + } + + Widget_->setSubwindowVisibility(window_index, + is_visible, + is_visible ? camera->getRenderTarget(camera_type, false) : nullptr); + } +} + +bool ASimWorldGameMode::IsWidgetSubwindowVisible(int window_index) +{ + return Widget_ ? Widget_->getSubwindowVisibility(window_index) != 0 : false; +} + +void ASimWorldGameMode::ToggleSubwindowVisibility(int window_index) +{ + GetSubWindowSettings().at(window_index).visible = !GetSubWindowSettings().at(window_index).visible; + UpdateWidgetSubwindowVisibility(); +} + +void ASimWorldGameMode::InputEventToggleSubwindow0() +{ + ToggleSubwindowVisibility(0); +} + +void ASimWorldGameMode::InputEventToggleSubwindow1() +{ + ToggleSubwindowVisibility(1); +} + +void ASimWorldGameMode::InputEventToggleSubwindow2() +{ + ToggleSubwindowVisibility(2); +} + +void ASimWorldGameMode::InputEventToggleAll() +{ + GetSubWindowSettings().at(0).visible = !GetSubWindowSettings().at(0).visible; + GetSubWindowSettings().at(1).visible = GetSubWindowSettings().at(2).visible = GetSubWindowSettings().at(0).visible; + UpdateWidgetSubwindowVisibility(); +} + +// ========================== Settings Helpers ========================== + +const std::vector& +ASimWorldGameMode::GetSubWindowSettings() const +{ + return AirSimSettings::singleton().subwindow_settings; +} + +std::vector& +ASimWorldGameMode::GetSubWindowSettings() +{ + return AirSimSettings::singleton().subwindow_settings; +} + +std::string ASimWorldGameMode::GetSimModeFromUser() +{ + if (EAppReturnType::No == UAirBlueprintLib::ShowMessage(EAppMsgType::YesNo, + "Would you like to use car simulation? Choose no to use quadrotor simulation.", + "Choose Vehicle")) { + return AirSimSettings::kSimModeTypeMultirotor; + } + else + return AirSimSettings::kSimModeTypeCar; +} + +FString ASimWorldGameMode::GetLaunchPath(const std::string& filename) +{ + FString launch_rel_path = FPaths::LaunchDir(); + FString abs_path = FPaths::ConvertRelativePathToFull(launch_rel_path); + return FPaths::Combine(abs_path, FString(filename.c_str())); +} + +bool ASimWorldGameMode::GetSettingsText(std::string& settingsText) +{ + return (GetSettingsTextFromCommandLine(settingsText) || + ReadSettingsTextFromFile(FString(msr::airlib::Settings::getExecutableFullPath("settings.json").c_str()), settingsText) || + ReadSettingsTextFromFile(GetLaunchPath("settings.json"), settingsText) || + ReadSettingsTextFromFile(FString(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json").c_str()), settingsText)); +} + +bool ASimWorldGameMode::GetSettingsTextFromCommandLine(std::string& settingsText) +{ + bool found = false; + FString settingsTextFString; + const TCHAR* commandLineArgs = FCommandLine::Get(); + + if (FParse::Param(commandLineArgs, TEXT("-settings"))) { + FString commandLineArgsFString = FString(commandLineArgs); + int idx = commandLineArgsFString.Find(TEXT("-settings")); + FString settingsJsonFString = commandLineArgsFString.RightChop(idx + 10); + + if (ReadSettingsTextFromFile(settingsJsonFString.TrimQuotes(), settingsText)) { + return true; + } + + if (FParse::QuotedString(*settingsJsonFString, settingsTextFString)) { + settingsText = std::string(TCHAR_TO_UTF8(*settingsTextFString)); + found = true; + } + } + + return found; +} + +bool ASimWorldGameMode::ReadSettingsTextFromFile(const FString& settingsFilepath, std::string& settingsText) +{ + bool found = FPaths::FileExists(settingsFilepath); + if (found) { + FString settingsTextFStr; + bool readSuccessful = FFileHelper::LoadFileToString(settingsTextFStr, *settingsFilepath); + if (readSuccessful) { + UAirBlueprintLib::LogMessageString("Loaded settings from ", + TCHAR_TO_UTF8(*settingsFilepath), + LogDebugLevel::Informational); + settingsText = TCHAR_TO_UTF8(*settingsTextFStr); + } + else { + UAirBlueprintLib::LogMessageString("Cannot read file ", + TCHAR_TO_UTF8(*settingsFilepath), + LogDebugLevel::Failure); + throw std::runtime_error("Cannot read settings file."); + } + } + return found; +} diff --git a/Unreal/Plugins/AirSim/Source/SimWorldGameMode.h b/Unreal/Plugins/AirSim/Source/SimWorldGameMode.h new file mode 100644 index 000000000..f25ded762 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/SimWorldGameMode.h @@ -0,0 +1,145 @@ +// SimWorldGameMode.h — Unified CARLA + AirSim GameMode +// Inherits from ACarlaGameModeBase (all CARLA features) and bootstraps +// AirSim's SimMode as a regular actor, avoiding the GameMode slot conflict. + +#pragma once + +#include "CoreMinimal.h" +#include "Carla/Game/CarlaGameModeBase.h" +#include "Carla/Weather/WeatherParameters.h" +#include "SimHUD/SimHUDWidget.h" +#include "SimMode/SimModeBase.h" +#include "PIPCamera.h" +#include "common/AirSimSettings.hpp" +#include "HAL/Runnable.h" +#include "HAL/RunnableThread.h" +#include "Widgets/SCompoundWidget.h" +#include "SimWorldGameMode.generated.h" + +// Forward declarations +class FDroneControlWorker; + +UCLASS() +class AIRSIM_API ASimWorldGameMode : public ACarlaGameModeBase +{ + GENERATED_BODY() + +public: + typedef msr::airlib::ImageCaptureBase::ImageType ImageType; + typedef msr::airlib::AirSimSettings AirSimSettings; + + ASimWorldGameMode(const FObjectInitializer& ObjectInitializer); + + virtual void BeginPlay() override; + virtual void Tick(float DeltaSeconds) override; + virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + + // Input event handlers (ported from SimHUD) + void InputEventToggleRecording(); + void InputEventToggleReport(); + void InputEventToggleHelp(); + void InputEventToggleTrace(); + void InputEventToggleSubwindow0(); + void InputEventToggleSubwindow1(); + void InputEventToggleSubwindow2(); + void InputEventToggleAll(); + + // FPS drone control input handlers + void InputEventNextWeather(); + void InputEventToggleMouseCapture(); + void InputEventSpeedUp(); + void InputEventSpeedDown(); + void InputEventToggleHelpOverlay(); + void InputEventTogglePhysicsMode(); + +protected: + void SetupAirSimInputBindings(); + void ToggleRecordHandler(); + void UpdateWidgetSubwindowVisibility(); + bool IsWidgetSubwindowVisible(int window_index); + void ToggleSubwindowVisibility(int window_index); + + // FPS drone control + void SetupFPSControl(); + void UpdateFPSControl(float DeltaSeconds); + void UpdateCameraFollow(); + void DrawHelpOverlay(); + void CreateHelpOverlayWidget(); + void ShowHelpOverlay(bool bShow); + void UpdateHelpOverlayText(); + +private: + void InitializeAirSimSettings(); + void SetUnrealEngineSettings(); + void CreateSimMode(); + void CreateAirSimWidget(); + void InitializeSubWindows(); + + bool GetSettingsText(std::string& settingsText); + bool GetSettingsTextFromCommandLine(std::string& settingsText); + bool ReadSettingsTextFromFile(const FString& fileName, std::string& settingsText); + std::string GetSimModeFromUser(); + + static FString GetLaunchPath(const std::string& filename); + + const std::vector& GetSubWindowSettings() const; + std::vector& GetSubWindowSettings(); + +private: + typedef common_utils::Utils Utils; + UClass* WidgetClass_; + + UPROPERTY() + USimHUDWidget* Widget_; + + UPROPERTY() + ASimModeBase* SimMode_; + + APIPCamera* SubwindowCameras_[AirSimSettings::kSubwindowCount]; + + // ---- FPS Drone Control State ---- + + // Drone speed & orientation + float DroneSpeed_ = 8.0f; // m/s, adjustable with +/- + float FPSYaw_ = 0.0f; // accumulated yaw (degrees) + float FPSPitch_ = 0.0f; // camera pitch (degrees), clamped ±89 + float CameraFollowYaw_ = 0.0f; // smoothed camera yaw for 3rd person + bool bMouseCaptured_ = true; // mouse capture state + bool bMouseInitialized_ = false; // first mouse position read done + bool bFPSControlActive_ = false; // FPS control initialized? + bool bYawInitialized_ = false; // FPSYaw_ synced from drone actual yaw? + + // Cached spectator + UPROPERTY() + APawn* CachedSpectator_ = nullptr; + + // Background thread for drone commands + FDroneControlWorker* DroneWorker_ = nullptr; + FRunnableThread* DroneThread_ = nullptr; + + // Shared state between game thread and drone control thread + FCriticalSection DroneControlLock_; + FVector DesiredVelocityNED_ = FVector::ZeroVector; // NED velocity + float DesiredYaw_ = 0.0f; + bool bShouldHover_ = true; // true when no movement input + bool bDroneReady_ = false; // set by worker after takeoff + + // Weather presets + TArray WeatherPresets_; + int32 WeatherIndex_ = 0; + + // One-time drone registration with CARLA ActorDispatcher + bool bDroneRegistered_ = false; + + // Help overlay & physics mode (v0.1.5) + bool bShowHelp_ = false; + bool bPhysicsCollision_ = true; // default: physics mode ON + + // Slate help overlay widget (Apple-style design) + TSharedPtr HelpOverlayContainer_; + TSharedPtr HelpTitleBlock_; + TSharedPtr HelpSubtitleBlock_; + TSharedPtr HelpContentBlock_; + TSharedPtr HelpStatusBlock_; + TSharedPtr HelpOverlayWrapper_; +}; \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp index 3a0272d19..79e63ebe9 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp @@ -35,7 +35,7 @@ msr::airlib::CarApiBase::CarState CarPawnApi::getCarState() const movement_->GetEngineMaxRotationSpeed(), last_controls_.handbrake, *pawn_kinematics_, - vehicle_api_->clock()->nowNanos()); + msr::airlib::ClockFactory::get()->nowNanos()); return state; } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp index 0144ad07f..09062ae12 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp @@ -28,7 +28,7 @@ void ASimModeCar::initializePauseState() void ASimModeCar::continueForTime(double seconds) { pause_period_start_ = ClockFactory::get()->nowNanos(); - pause_period_ = seconds * current_clockspeed_; + pause_period_ = seconds; pause(false); } @@ -41,8 +41,6 @@ void ASimModeCar::continueForFrames(uint32_t frames) void ASimModeCar::setupClockSpeed() { - Super::setupClockSpeed(); - current_clockspeed_ = getSettings().clock_speed; //setup clock in PhysX @@ -54,9 +52,6 @@ void ASimModeCar::Tick(float DeltaSeconds) { Super::Tick(DeltaSeconds); - if (!isPaused()) - ClockFactory::get()->stepBy(DeltaSeconds); - if (pause_period_start_ > 0) { if (ClockFactory::get()->elapsedSince(pause_period_start_) >= pause_period_) { if (!isPaused()) diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/ComputerVisionPawn.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/ComputerVisionPawn.cpp index e3a6e99c9..51593cc55 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/ComputerVisionPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/ComputerVisionPawn.cpp @@ -113,7 +113,7 @@ void AComputerVisionPawn::Tick(float Delta) pawn_events_.getPawnTickSignal().emit(Delta); //update ground level - if (manual_pose_controller_->getActor() == this) { + if (manual_pose_controller_ && manual_pose_controller_->getActor() == this) { manual_pose_controller_->updateActorPose(Delta); } } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp index 0f2700906..38a4f6ce6 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp @@ -2,6 +2,7 @@ #include "AirBlueprintLib.h" #include "vehicles/multirotor/MultiRotorParamsFactory.hpp" #include "UnrealSensors/UnrealSensorFactory.h" +#include "common/ClockFactory.hpp" #include using namespace msr::airlib; @@ -106,8 +107,41 @@ void MultirotorPawnSimApi::updateRendering(float dt) PawnSimApi::setPose(last_phys_pose_, pending_pose_collisions_); pending_pose_status_ = PendingPoseStatus::NonePending; } - else - PawnSimApi::setPose(last_phys_pose_, false); + else { + // CarlaAir v0.1.6: Use sweep mode when physics collision is ON, teleport when OFF. + // Sweep mode = UE4 native collision: drone stops at walls naturally. + // Teleport mode = pass through everything (invincible/noclip). + // Ground clamp always active to prevent falling through CARLA terrain. + if (bPhysicsCollisionEnabled_) { + PawnSimApi::setPose(last_phys_pose_, false); // sweep mode: UE4 detects collisions + } + else { + PawnSimApi::setPose(last_phys_pose_, true); // teleport mode: pass through + } + } + + // Ground clamp: prevent drone from falling through CARLA terrain/roads. + // This fixes the false-collision issue where sweep mode would trap the drone + // inside terrain geometry. By ensuring the drone is always above ground, + // sweep mode only triggers on actual buildings/obstacles. + APawn* pawn = getPawn(); + if (pawn && pawn->GetWorld()) { + FVector DronePos = pawn->GetActorLocation(); + FHitResult GroundHit; + FCollisionQueryParams GroundParams(SCENE_QUERY_STAT(GroundClamp), true, pawn); + if (pawn->GetWorld()->LineTraceSingleByChannel( + GroundHit, + DronePos + FVector(0, 0, 10000.0f), + DronePos - FVector(0, 0, 500.0f), + ECC_WorldStatic, + GroundParams)) { + float GroundZ = GroundHit.ImpactPoint.Z; + if (DronePos.Z < GroundZ + MIN_GROUND_CLEARANCE) { + DronePos.Z = GroundZ + MIN_GROUND_CLEARANCE; + pawn->SetActorLocation(DronePos, false, nullptr, ETeleportType::TeleportPhysics); + } + } + } } //UAirBlueprintLib::LogMessage(TEXT("Collision (raw) Count:"), FString::FromInt(collision_response.collision_count_raw), LogDebugLevel::Unimportant); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h index f7db8a8fd..e284a4464 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h @@ -56,7 +56,24 @@ class MultirotorPawnSimApi : public PawnSimApi return vehicle_api_.get(); } + // Physics collision mode: true = bounce off buildings + ground clamp, false = pass through + void setPhysicsCollisionEnabled(bool enabled) { bPhysicsCollisionEnabled_ = enabled; } + bool isPhysicsCollisionEnabled() const { return bPhysicsCollisionEnabled_; } + bool isBouncing() const { return BounceTimeRemaining_ > 0.0f; } + FVector getBounceVelocity() const { return BounceVelocityUU_; } + private: + bool bPhysicsCollisionEnabled_ = true; + + // Bounce-back state for realistic collision response + FVector BounceVelocityUU_ = FVector::ZeroVector; // UU velocity during bounce + float BounceTimeRemaining_ = 0.0f; // seconds left in bounce animation + float BounceCooldown_ = 0.0f; // cooldown to prevent repeated hits + static constexpr float BOUNCE_DURATION = 0.4f; // bounce animation duration (seconds) + static constexpr float BOUNCE_COOLDOWN = 0.3f; // min time between bounces + static constexpr float BOUNCE_RESTITUTION = 0.6f; // energy retention on bounce (0-1) + static constexpr float TRACE_DISTANCE = 120.0f; // collision probe distance (cm, ~1.2m) + static constexpr float MIN_GROUND_CLEARANCE = 5.0f; // cm above ground std::unique_ptr vehicle_api_; std::unique_ptr vehicle_params_; @@ -65,7 +82,7 @@ class MultirotorPawnSimApi : public PawnSimApi std::vector rotor_actuator_info_; //show info on collision response from physics engine - CollisionResponse collision_response; + msr::airlib::CollisionResponse collision_response; MultirotorPawnEvents* pawn_events_; @@ -85,5 +102,5 @@ class MultirotorPawnSimApi : public PawnSimApi Pose last_phys_pose_; //for trace lines showing vehicle path std::vector vehicle_api_messages_; - RotorStates rotor_states_; + msr::airlib::RotorStates rotor_states_; }; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 6e289a190..b3beb4ab1 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -1039,8 +1039,9 @@ std::vector WorldSimApi::getDetections(ImageCaptureB result[i].name = std::string(TCHAR_TO_UTF8(*(detections[i].Actor->GetFName().ToString()))); Vector3r nedWrtOrigin = ned_transform.toGlobalNed(detections[i].Actor->GetActorLocation()); - result[i].geo_point = msr::airlib::EarthUtils::nedToGeodetic(nedWrtOrigin, - AirSimSettings::singleton().origin_geopoint); + result[i].geo_point = msr::airlib::EarthUtils::nedToGeodetic( + nedWrtOrigin, + msr::airlib::AirSimSettings::singleton().origin_geopoint); result[i].box2D.min = Vector2r(detections[i].Box2D.Min.X, detections[i].Box2D.Min.Y); result[i].box2D.max = Vector2r(detections[i].Box2D.Max.X, detections[i].Box2D.Max.Y); diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index a6883cda2..8d5c4869e 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -7,6 +7,7 @@ #include "SimMode/SimModeBase.h" #include "Components/StaticMeshComponent.h" #include "Runtime/Engine/Classes/Engine/StaticMesh.h" +#include "Engine/LevelStreamingDynamic.h" #include class WorldSimApi : public msr::airlib::WorldSimApiBase @@ -14,6 +15,8 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase public: typedef msr::airlib::Pose Pose; typedef msr::airlib::Vector3r Vector3r; + typedef msr::airlib::Vector2r Vector2r; + typedef msr::airlib::Quaternionr Quaternionr; typedef msr::airlib::MeshPositionVertexBuffersResponse MeshPositionVertexBuffersResponse; typedef msr::airlib::ImageCaptureBase ImageCaptureBase; typedef msr::airlib::CameraDetails CameraDetails; @@ -125,5 +128,6 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase private: ASimModeBase* simmode_; + ULevelStreamingDynamic* current_level_; std::vector voxel_grid_; };