diff --git a/CHANGELOG.md b/CHANGELOG.md index c7e9d15..737d612 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,10 +1,14 @@ -## DReyeVR 0.1.2 (for Carla 0.9.13) +## DReyeVR 0.1.3 +- Fix bug where other non-ego Autopilto vehicles ignored the EgoVehicle and would routinely crash into it +- Fix bug where some EgoVehicle attributes were missing, notably `"number_of_wheels"` + +## DReyeVR 0.1.2 - Update documentation to refer to CarlaUnreal UE4 fork rather than HarpLab fork - Apply patches for installation of zlib (broken link) and xerces (broken link & `-Dtranscoder=windows` flag) and PythonAPI - Fix crash on BeginPlay when EgoVehicle blueprint is already present in map (before world start) - Prefer building PythonAPI with `python` rather than Windows built-in `py -3` (good if using conda) but fallback if `python` is not available -## DReyeVR 0.1.1 (for Carla 0.9.13) +## DReyeVR 0.1.1 - Update documentation, add developer-centric in-depth documentation - Adding missing includes for TrafficSign RoadInfoSignal in SignComponent - Replacing `DReyeVRData.inl` with `DReyeVRData.cpp` and corresponding virtual classes @@ -15,7 +19,7 @@ - Fix bug with spectator being reenacted by Carla, preferring to use our own ASpectator - Automatically possess spectator pawn when EgoVehicle is destroyed. Can re-spawn EgoVehicle by pressing 1. -## DReyeVR 0.1.0 (for Carla 0.9.13) +## DReyeVR 0.1.0 - Replace existing `LevelScript` (`ADReyeVRLevel`) with `GameMode` (`ADReyeVRGameMode`). This allows us to not need to carry the (large) map blueprint files (ue4 binary) and we can use the vanilla Carla maps without modification. By default we spawn the EgoVehicle in the first of the recommended Carla locations, but this default behavior can be changed in the PythonAPI. For instance, you can delay spawning the EgoVehicle until via PythonAPI where you can specify the spawn transform. Existing functionality is preserved using `find_ego_vehicle` and `find_ego_sensor` which spawn the DReyeVR EgoVehicle if it does not exist in the world. - Added `ADReyeVRFactory` as the Carla-esque spawning and registry functionality so the `EgoVehicle` and `EgoSensor` are spawned with the same "Factory" mechanisms as existing Carla vehicles/sensors/props/etc. - Renamed DReyeVR-specific actors to be addressible in PythonAPI as `"harplab.dreyevr_$X.$id"` such as `"harplab.dreyevr_vehicle.model3"` and `"harplab.dreyevr_sensor.ego_sensor"`. Avoids conflicts with existing Carla PythonAPI scripts that may filter on `"vehicle.*"`. diff --git a/Carla/Recorder/CarlaReplayer.cpp b/Carla/Recorder/CarlaReplayer.cpp index 61a8568..494038d 100644 --- a/Carla/Recorder/CarlaReplayer.cpp +++ b/Carla/Recorder/CarlaReplayer.cpp @@ -32,6 +32,10 @@ void CarlaReplayer::Stop(bool bKeepActors) // callback Helper.ProcessReplayerFinish(bKeepActors, IgnoreHero, IsHeroMap); + + // turn off DReyeVR replay + if (GetEgoSensor()) + GetEgoSensor()->StopReplaying(); } File.close(); @@ -291,6 +295,70 @@ void CarlaReplayer::CheckPlayAfterMapLoaded(void) Enabled = true; } +class ADReyeVRSensor *CarlaReplayer::GetEgoSensor() +{ + if (EgoSensor.IsValid()) { + return EgoSensor.Get(); + } + // not tracked yet, lets find the EgoSensor + if (Episode == nullptr) { + DReyeVR_LOG_ERROR("No Replayer Episode available!"); + return nullptr; + } + EgoSensor = ADReyeVRSensor::GetDReyeVRSensor(Episode->GetWorld()); + if (!EgoSensor.IsValid()) { + DReyeVR_LOG_ERROR("No EgoSensor available!"); + return nullptr; + } + return EgoSensor.Get(); +} + +template<> +void CarlaReplayer::ProcessDReyeVR(double Per, double DeltaTime) +{ + uint16_t Total; + // read number of DReyeVR entries + ReadValue(File, Total); // read number of events + check(Total == 1); // should be only one Agg data + for (uint16_t i = 0; i < Total; ++i) + { + struct DReyeVRDataRecorder Instance; + Instance.Read(File); + Helper.ProcessReplayerDReyeVR(GetEgoSensor(), Instance.Data, Per); + } +} + +template<> +void CarlaReplayer::ProcessDReyeVR(double Per, double DeltaTime) +{ + uint16_t Total; + ReadValue(File, Total); // read number of events + CustomActorsVisited.clear(); + for (uint16_t i = 0; i < Total; ++i) + { + struct DReyeVRDataRecorder Instance; + Instance.Read(File); + Helper.ProcessReplayerDReyeVR(GetEgoSensor(), Instance.Data, Per); + auto Name = Instance.GetUniqueName(); + CustomActorsVisited.insert(Name); // to track lifetime + } + + for (auto It = ADReyeVRCustomActor::ActiveCustomActors.begin(); It != ADReyeVRCustomActor::ActiveCustomActors.end();){ + const std::string &ActiveActorName = It->first; + if (CustomActorsVisited.find(ActiveActorName) == CustomActorsVisited.end()) // currently alive actor who was not visited... time to disable + { + // now this has to be garbage collected + auto Next = std::next(It, 1); // iterator following the last removed element + It->second->Deactivate(); + It = Next; + } + else + { + ++It; // increment iterator if not erased + } + } +} + void CarlaReplayer::ProcessToTime(double Time, bool IsFirstTime) { double Per = 0.0f; @@ -401,18 +469,18 @@ void CarlaReplayer::ProcessToTime(double Time, bool IsFirstTime) ProcessWeather(); break; - // DReyeVR eye logging data + // DReyeVR ego sensor data case static_cast(CarlaRecorderPacketId::DReyeVR): if (bFrameFound) - ProcessDReyeVRData>(Per, Time, true); + ProcessDReyeVR(Per, Time); else SkipPacket(); break; - // DReyeVR eye logging data + // DReyeVR custom actor data case static_cast(CarlaRecorderPacketId::DReyeVRCustomActor): if (bFrameFound) - ProcessDReyeVRData>(Per, Time, false); + ProcessDReyeVR(Per, Time); else SkipPacket(); break; @@ -647,49 +715,6 @@ void CarlaReplayer::ProcessWeather(void) } } -template void CarlaReplayer::ProcessDReyeVRData(double Per, double DeltaTime, bool bShouldBeOnlyOne) -{ - uint16_t Total; - // custom DReyeVR packets - - // read Total DReyeVR events - ReadValue(File, Total); // read number of events - - Visited.clear(); - for (uint16_t i = 0; i < Total; ++i) - { - T DReyeVRDataInstance; - DReyeVRDataInstance.Read(File); - Helper.ProcessReplayerDReyeVRData(DReyeVRDataInstance, Per); - if (!bShouldBeOnlyOne) - { - auto Name = DReyeVRDataInstance.GetUniqueName(); - Visited.insert(Name); - } - } - if (bShouldBeOnlyOne) - { - check(Total == 1); - } - else - { - for (auto It = ADReyeVRCustomActor::ActiveCustomActors.begin(); It != ADReyeVRCustomActor::ActiveCustomActors.end();){ - const std::string &ActiveActorName = It->first; - if (Visited.find(ActiveActorName) == Visited.end()) // currently alive actor who was not visited... time to disable - { - // now this has to be garbage collected - auto Next = std::next(It, 1); // iterator following the last removed element - It->second->Deactivate(); - It = Next; - } - else - { - ++It; // increment iterator if not erased - } - } - } -} - void CarlaReplayer::ProcessPositions(bool IsFirstTime) { uint16_t i, Total; @@ -823,12 +848,8 @@ void CarlaReplayer::ProcessFrameByFrame() if (SyncCurrentFrameId > 0) LastTime = FrameStartTimes[SyncCurrentFrameId - 1]; ProcessToTime(FrameStartTimes[SyncCurrentFrameId] - LastTime, (SyncCurrentFrameId == 0)); - if (ADReyeVRSensor::GetDReyeVRSensor(Episode->GetWorld())) - // have the vehicle camera take a screenshot to record the replay - ADReyeVRSensor::GetDReyeVRSensor()->TakeScreenshot(); - else - DReyeVR_LOG_ERROR("No DReyeVR sensor available!"); - + if (GetEgoSensor()) // take screenshot of this frame + GetEgoSensor()->TakeScreenshot(); // progress to the next frame if (SyncCurrentFrameId < FrameStartTimes.size() - 1) SyncCurrentFrameId++; diff --git a/Carla/Recorder/CarlaReplayer.h b/Carla/Recorder/CarlaReplayer.h index 9002013..0331b8c 100644 --- a/Carla/Recorder/CarlaReplayer.h +++ b/Carla/Recorder/CarlaReplayer.h @@ -168,8 +168,11 @@ class CARLA_API CarlaReplayer void ProcessWeather(void); // DReyeVR recordings - template void ProcessDReyeVRData(double Per, double DeltaTime, bool bShouldBeOnlyOne); - std::unordered_set Visited = {}; + template + void ProcessDReyeVR(double Per, double DeltaTime); + std::unordered_set CustomActorsVisited = {}; + class ADReyeVRSensor *GetEgoSensor(); // (safe) getter for EgoSensor + TWeakObjectPtr EgoSensor; // For restarting the recording with the same params struct LastReplayStruct diff --git a/Carla/Recorder/CarlaReplayerHelper.cpp b/Carla/Recorder/CarlaReplayerHelper.cpp index b7130f9..82afabb 100644 --- a/Carla/Recorder/CarlaReplayerHelper.cpp +++ b/Carla/Recorder/CarlaReplayerHelper.cpp @@ -477,20 +477,20 @@ bool CarlaReplayerHelper::ProcessReplayerFinish(bool bApplyAutopilot, bool bIgno break; } } - // tell the DReyeVR sensor to NOT continue replaying - if (ADReyeVRSensor::GetDReyeVRSensor(Episode->GetWorld())) - ADReyeVRSensor::GetDReyeVRSensor()->StopReplaying(); - else - DReyeVR_LOG_ERROR("No DReyeVR sensor available!"); return true; } -template void CarlaReplayerHelper::ProcessReplayerDReyeVRData(const T &DReyeVRDataInstance, const double Per) +template +void CarlaReplayerHelper::ProcessReplayerDReyeVR(ADReyeVRSensor *EgoSensor, const T &Data, const double Per) { - if (ADReyeVRSensor::GetDReyeVRSensor(Episode->GetWorld())) - ADReyeVRSensor::GetDReyeVRSensor()->UpdateData(DReyeVRDataInstance.Data, Per); - else + if (EgoSensor == nullptr) { // try getting and assigning the new EgoSensor + EgoSensor = ADReyeVRSensor::GetDReyeVRSensor(Episode->GetWorld()); + } + if (EgoSensor == nullptr) { // still null?? throw an error DReyeVR_LOG_ERROR("No DReyeVR sensor available!"); + return; + } + EgoSensor->UpdateData(Data, Per); } void CarlaReplayerHelper::SetActorVelocity(FCarlaActor *CarlaActor, FVector Velocity) diff --git a/Carla/Recorder/CarlaReplayerHelper.h b/Carla/Recorder/CarlaReplayerHelper.h index 9e78129..b4695f4 100644 --- a/Carla/Recorder/CarlaReplayerHelper.h +++ b/Carla/Recorder/CarlaReplayerHelper.h @@ -23,6 +23,7 @@ class UCarlaEpisode; class FCarlaActor; struct FActorDescription; +class ADReyeVRSensor; // fwd for DReyeVR (avoid header conflict) class CarlaReplayerHelper { @@ -75,7 +76,8 @@ class CarlaReplayerHelper bool ProcessReplayerFinish(bool bApplyAutopilot, bool bIgnoreHero, std::unordered_map &IsHero); // update the DReyeVR ego sensor and custom types - template void ProcessReplayerDReyeVRData(const T &DReyeVRDataInstance, const double Per); + template + void ProcessReplayerDReyeVR(ADReyeVRSensor *EgoSensor, const T &Data, const double Per); // set the camera position to follow an actor bool SetCameraPosition(uint32_t Id, FVector Offset, FQuat Rotation); diff --git a/DReyeVR/DReyeVRFactory.cpp b/DReyeVR/DReyeVRFactory.cpp index 740d296..ca4d641 100644 --- a/DReyeVR/DReyeVRFactory.cpp +++ b/DReyeVR/DReyeVRFactory.cpp @@ -3,6 +3,7 @@ #include "Carla/Actor/ActorBlueprintFunctionLibrary.h" // UActorBlueprintFunctionLibrary #include "Carla/Actor/VehicleParameters.h" // FVehicleParameters #include "Carla/Game/CarlaEpisode.h" // UCarlaEpisode +#include "DReyeVRGameMode.h" // ADReyeVRGameMode #include "EgoSensor.h" // AEgoSensor #include "EgoVehicle.h" // AEgoVehicle @@ -63,46 +64,28 @@ void ADReyeVRFactory::MakeVehicleDefinition(const FVehicleParameters &Parameters Definition = MakeGenericDefinition(CATEGORY, TEXT("DReyeVR_Vehicle"), Parameters.Model); Definition.Class = Parameters.Class; - FActorVariation ActorRole; - { - ActorRole.Id = TEXT("role_name"); - ActorRole.Type = EActorAttributeType::String; - ActorRole.RecommendedValues = {TEXT("ego_vehicle")}; // assume this is the CARLA "hero" - ActorRole.bRestrictToRecommended = false; - } - Definition.Variations.Emplace(ActorRole); - - FActorVariation StickyControl; - { - StickyControl.Id = TEXT("sticky_control"); - StickyControl.Type = EActorAttributeType::Bool; - StickyControl.bRestrictToRecommended = false; - StickyControl.RecommendedValues.Emplace(TEXT("false")); - } - Definition.Variations.Emplace(StickyControl); + FActorAttribute ActorRole; + ActorRole.Id = "role_name"; + ActorRole.Type = EActorAttributeType::String; + ActorRole.Value = "hero"; + Definition.Attributes.Emplace(ActorRole); FActorAttribute ObjectType; - { - ObjectType.Id = TEXT("object_type"); - ObjectType.Type = EActorAttributeType::String; - ObjectType.Value = Parameters.ObjectType; - } + ObjectType.Id = "object_type"; + ObjectType.Type = EActorAttributeType::String; + ObjectType.Value = Parameters.ObjectType; Definition.Attributes.Emplace(ObjectType); FActorAttribute NumberOfWheels; - { - NumberOfWheels.Id = TEXT("number_of_wheels"); - NumberOfWheels.Type = EActorAttributeType::Int; - NumberOfWheels.Value = FString::FromInt(Parameters.NumberOfWheels); - } + NumberOfWheels.Id = "number_of_wheels"; + NumberOfWheels.Type = EActorAttributeType::Int; + NumberOfWheels.Value = FString::FromInt(Parameters.NumberOfWheels); Definition.Attributes.Emplace(NumberOfWheels); FActorAttribute Generation; - { - Generation.Id = TEXT("generation"); - Generation.Type = EActorAttributeType::Int; - Generation.Value = FString::FromInt(Parameters.Generation); - } + Generation.Id = "generation"; + Generation.Type = EActorAttributeType::Int; + Generation.Value = FString::FromInt(Parameters.Generation); Definition.Attributes.Emplace(Generation); } @@ -156,6 +139,11 @@ FActorSpawnResult ADReyeVRFactory::SpawnActor(const FTransform &SpawnAtTransform // EgoVehicle needs the special EgoVehicleBPClass since they depend on the EgoVehicle Blueprint return World->SpawnActor(EgoVehicleBPClass, SpawnAtTransform, SpawnParameters); }); + + // update the GameMode's EgoVehicle in case it was spawned by someone else + auto *Game = Cast(UGameplayStatics::GetGameMode(World)); + if (Game != nullptr) + Game->SetEgoVehicle(Cast(SpawnedActor)); } else if (ActorDescription.Class == AEgoSensor::StaticClass()) { diff --git a/DReyeVR/DReyeVRGameMode.cpp b/DReyeVR/DReyeVRGameMode.cpp index 24af619..5883399 100644 --- a/DReyeVR/DReyeVRGameMode.cpp +++ b/DReyeVR/DReyeVRGameMode.cpp @@ -72,7 +72,7 @@ void ADReyeVRGameMode::BeginPlay() Super::BeginPlay(); // Initialize player - Player = UGameplayStatics::GetPlayerController(GetWorld(), 0); + GetPlayer(); // Can we tick? SetActorTickEnabled(false); // make sure we do not tick ourselves @@ -85,16 +85,27 @@ void ADReyeVRGameMode::BeginPlay() // spawn the DReyeVR pawn and possess it (first) SetupDReyeVRPawn(); + ensure(GetPawn() != nullptr); // Initialize the DReyeVR EgoVehicle and Sensor (second) - SetupEgoVehicle(); + if (bDoSpawnEgoVehicle) + { + SetupEgoVehicle(); + ensure(GetEgoVehicle() != nullptr); + } // Initialize DReyeVR spectator (third) SetupSpectator(); + ensure(GetSpectator() != nullptr); } void ADReyeVRGameMode::SetupDReyeVRPawn() { + if (DReyeVR_Pawn.IsValid()) + { + LOG("Not spawning new DReyeVR pawn"); + return; + } FActorSpawnParameters SpawnParams; SpawnParams.Owner = this; SpawnParams.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AlwaysSpawn; @@ -102,30 +113,37 @@ void ADReyeVRGameMode::SetupDReyeVRPawn() /// NOTE: the pawn is automatically possessed by player0 // as the constructor has the AutoPossessPlayer != disabled // if you want to manually possess then you can do Player->Possess(DReyeVR_Pawn); - if (DReyeVR_Pawn == nullptr) + if (!DReyeVR_Pawn.IsValid()) { LOG_ERROR("Unable to spawn DReyeVR pawn!") } else { - DReyeVR_Pawn->BeginPlayer(Player); + DReyeVR_Pawn.Get()->BeginPlayer(GetPlayer()); LOG("Successfully spawned DReyeVR pawn"); } } +void ADReyeVRGameMode::SetEgoVehicle(AEgoVehicle *Ego) +{ + EgoVehiclePtr.Reset(); + EgoVehiclePtr = Ego; + check(EgoVehiclePtr.IsValid()); + ensure(GetPawn() != nullptr); // also respawn DReyeVR pawn if needed + // assign the (possibly new) EgoVehicle to the pawn + if (GetPawn() != nullptr) + { + DReyeVR_Pawn.Get()->BeginEgoVehicle(EgoVehiclePtr.Get(), GetWorld()); + } +} + bool ADReyeVRGameMode::SetupEgoVehicle() { - if (EgoVehiclePtr != nullptr || bDoSpawnEgoVehicle == false) + if (EgoVehiclePtr.IsValid()) { LOG("Not spawning new EgoVehicle"); - if (EgoVehiclePtr == nullptr) - { - LOG("EgoVehicle unavailable, possessing spectator by default") - PossessSpectator(); // NOTE: need to call SetupSpectator before this! - } return true; } - ensure(DReyeVR_Pawn); TArray FoundActors; UGameplayStatics::GetAllActorsOfClass(GetWorld(), AEgoVehicle::StaticClass(), FoundActors); @@ -148,37 +166,53 @@ bool ADReyeVRGameMode::SetupEgoVehicle() } // finalize the EgoVehicle by installing the DReyeVR_Pawn to control it - check(EgoVehiclePtr != nullptr); - return (EgoVehiclePtr != nullptr); + return (EgoVehiclePtr.IsValid()); } void ADReyeVRGameMode::SetupSpectator() { - if (bUseCarlaSpectator) - { // look for existing spectator in world - UCarlaEpisode *Episode = UCarlaStatics::GetCurrentEpisode(GetWorld()); - if (Episode != nullptr) - SpectatorPtr = Episode->GetSpectatorPawn(); - else if (Player != nullptr) - { - SpectatorPtr = Player->GetPawn(); - } + if (SpectatorPtr.IsValid()) + { + LOG("Not spawning new Spectator"); + return; } - // spawn if necessary - if (SpectatorPtr != nullptr) + // always disable the Carla spectator from DReyeVR use + UCarlaEpisode *Episode = UCarlaStatics::GetCurrentEpisode(GetWorld()); + APawn *CarlaSpectator = nullptr; + if (Episode != nullptr) { - LOG("Found available spectator in world"); + CarlaSpectator = Episode->GetSpectatorPawn(); + if (CarlaSpectator != nullptr) + CarlaSpectator->SetActorHiddenInGame(true); } - else + + // whether or not to use Carla spectator + if (bUseCarlaSpectator) + { + if (CarlaSpectator != nullptr) + SpectatorPtr = CarlaSpectator; + else if (GetPlayer() != nullptr) + SpectatorPtr = Player.Get()->GetPawn(); + } + + // spawn the Spectator pawn { LOG("Spawning DReyeVR Spectator Pawn in the world"); FVector SpawnLocn; FRotator SpawnRotn; - if (EgoVehiclePtr != nullptr) + if (EgoVehiclePtr.IsValid()) + { + SpawnLocn = EgoVehiclePtr.Get()->GetCameraPosn(); + SpawnRotn = EgoVehiclePtr.Get()->GetCameraRot(); + } + else { - SpawnLocn = EgoVehiclePtr->GetCameraPosn(); - SpawnRotn = EgoVehiclePtr->GetCameraRot(); + // spawn above the vehicle recommended spawn pt + FTransform RecommendedPt = GetSpawnPoint(); + SpawnLocn = RecommendedPt.GetLocation(); + SpawnLocn.Z += 10.f * 100.f; // up in the air 10m ish + SpawnRotn = RecommendedPt.Rotator(); } // create new spectator pawn FActorSpawnParameters SpawnParams; @@ -189,27 +223,57 @@ void ADReyeVRGameMode::SetupSpectator() SpawnLocn, SpawnRotn, SpawnParams); } - if (SpectatorPtr) + if (SpectatorPtr.IsValid()) { - SpectatorPtr->SetActorHiddenInGame(true); // make spectator invisible - SpectatorPtr->GetRootComponent()->DestroyPhysicsState(); // no physics (just no-clip) - SpectatorPtr->SetActorEnableCollision(false); // no collisions + SpectatorPtr.Get()->SetActorHiddenInGame(true); // make spectator invisible + SpectatorPtr.Get()->GetRootComponent()->DestroyPhysicsState(); // no physics (just no-clip) + SpectatorPtr.Get()->SetActorEnableCollision(false); // no collisions LOG("Successfully initiated spectator actor"); } + + // automatically possess the spectator ptr if no ego vehicle present! + if (!EgoVehiclePtr.IsValid()) + { + PossessSpectator(); + } +} + +APawn *ADReyeVRGameMode::GetSpectator() +{ + return SafePtrGet("Spectator", SpectatorPtr, [&](void) { SetupSpectator(); }); +} + +AEgoVehicle *ADReyeVRGameMode::GetEgoVehicle() +{ + return SafePtrGet("EgoVehicle", EgoVehiclePtr, [&](void) { SetupEgoVehicle(); }); +} + +APlayerController *ADReyeVRGameMode::GetPlayer() +{ + return SafePtrGet("Player", Player, + [&](void) { Player = GetWorld()->GetFirstPlayerController(); }); +} + +ADReyeVRPawn *ADReyeVRGameMode::GetPawn() +{ + return SafePtrGet("Pawn", DReyeVR_Pawn, [&](void) { SetupDReyeVRPawn(); }); } void ADReyeVRGameMode::BeginDestroy() { Super::BeginDestroy(); - if (DReyeVR_Pawn) - DReyeVR_Pawn->Destroy(); + if (DReyeVR_Pawn.IsValid()) + DReyeVR_Pawn.Get()->Destroy(); + DReyeVR_Pawn = nullptr; // release object and assign to null - if (EgoVehiclePtr) - EgoVehiclePtr->Destroy(); + if (EgoVehiclePtr.IsValid()) + EgoVehiclePtr.Get()->Destroy(); + EgoVehiclePtr = nullptr; - if (SpectatorPtr) - SpectatorPtr->Destroy(); + if (SpectatorPtr.IsValid()) + SpectatorPtr.Get()->Destroy(); + SpectatorPtr = nullptr; LOG("DReyeVRGameMode has been destroyed"); } @@ -229,11 +293,14 @@ void ADReyeVRGameMode::Tick(float DeltaSeconds) void ADReyeVRGameMode::SetupPlayerInputComponent() { + if (GetPlayer() == nullptr) + return; + check(Player.IsValid()); InputComponent = NewObject(this); InputComponent->RegisterComponent(); // set up gameplay key bindings check(InputComponent); - Player->PushInputComponent(InputComponent); // enable this InputComponent with the PlayerController + Player.Get()->PushInputComponent(InputComponent); // enable this InputComponent with the PlayerController // InputComponent->BindAction("ToggleCamera", IE_Pressed, this, &ADReyeVRGameMode::ToggleSpectator); InputComponent->BindAction("PlayPause_DReyeVR", IE_Pressed, this, &ADReyeVRGameMode::ReplayPlayPause); InputComponent->BindAction("FastForward_DReyeVR", IE_Pressed, this, &ADReyeVRGameMode::ReplayFastForward); @@ -249,98 +316,68 @@ void ADReyeVRGameMode::SetupPlayerInputComponent() void ADReyeVRGameMode::PossessEgoVehicle() { - if (EgoVehiclePtr == nullptr) - { - LOG_WARN("No EgoVehicle to possess! Attempting to remedy..."); - SetupEgoVehicle(); - if (EgoVehiclePtr == nullptr) - { - LOG_ERROR("Remedy failed, unable to possess EgoVehicle"); - return; - } - } - EgoVehiclePtr->SetAutopilot(false); + if (GetEgoVehicle() == nullptr || GetPawn() == nullptr || GetPlayer() == nullptr) + return; + + check(EgoVehiclePtr.IsValid()); + check(DReyeVR_Pawn.IsValid()); + check(Player.IsValid()); + EgoVehiclePtr.Get()->SetAutopilot(false); - if (DReyeVR_Pawn == nullptr) + // check if already possessing EgoVehicle (DReyeVRPawn) + if (Player.Get()->GetPawn() == DReyeVR_Pawn.Get()) { - LOG_WARN("No DReyeVR pawn to possess EgoVehicle! Attempting to remedy..."); - SetupDReyeVRPawn(); - if (DReyeVR_Pawn == nullptr) - { - LOG_ERROR("Remedy failed, unable to possess EgoVehicle"); - return; - } + LOG("Already possessing EgoVehicle"); return; } - { // check if already possessing EgoVehicle (DReyeVRPawn) - ensure(Player != nullptr); - if (Player->GetPawn() == DReyeVR_Pawn) - { - LOG("Currently possessing EgoVehicle"); - return; - } - } - LOG("Possessing DReyeVR EgoVehicle"); - Player->Possess(DReyeVR_Pawn); + Player.Get()->Possess(DReyeVR_Pawn.Get()); + DReyeVR_Pawn.Get()->BeginEgoVehicle(EgoVehiclePtr.Get(), GetWorld()); } void ADReyeVRGameMode::PossessSpectator() { - if (SpectatorPtr == nullptr) - { - LOG_WARN("No spectator to possess! Attempting to remedy..."); - SetupSpectator(); - if (SpectatorPtr == nullptr) - { - LOG_ERROR("Remedy failed, unable to possess spectator"); - return; - } - } + if (GetSpectator() == nullptr || GetPlayer() == nullptr) + return; - { // check if already possessing spectator - ensure(Player != nullptr); - if (Player->GetPawn() == SpectatorPtr) - { - LOG("Already possessing Spectator"); - return; - } + check(SpectatorPtr.IsValid()); + check(Player.IsValid()); + + // check if already possessing spectator + if (Player.Get()->GetPawn() == SpectatorPtr.Get()) + { + LOG("Already possessing Spectator"); + return; } - if (EgoVehiclePtr) + if (EgoVehiclePtr.IsValid()) { // spawn from EgoVehicle head position - const FVector &EgoLocn = EgoVehiclePtr->GetCameraPosn(); - const FRotator &EgoRotn = EgoVehiclePtr->GetCameraRot(); - SpectatorPtr->SetActorLocationAndRotation(EgoLocn, EgoRotn); + const FVector &EgoLocn = EgoVehiclePtr.Get()->GetCameraPosn(); + const FRotator &EgoRotn = EgoVehiclePtr.Get()->GetCameraRot(); + SpectatorPtr.Get()->SetActorLocationAndRotation(EgoLocn, EgoRotn); } // repossess the ego vehicle - Player->Possess(SpectatorPtr); + Player.Get()->Possess(SpectatorPtr.Get()); LOG("Possessing spectator player"); } void ADReyeVRGameMode::HandoffDriverToAI() { - if (EgoVehiclePtr == nullptr) - { - LOG_WARN("No EgoVehicle to handoff AI control! Attempting to remedy..."); - SetupEgoVehicle(); - if (EgoVehiclePtr == nullptr) - { - LOG_ERROR("Remedy failed, unable to access EgoVehicle"); - return; - } - } + if (GetEgoVehicle() == nullptr) + return; + + check(EgoVehiclePtr.IsValid()); { // check if autopilot already enabled - if (EgoVehiclePtr->GetAutopilotStatus() == true) + if (EgoVehiclePtr.Get()->GetAutopilotStatus() == true) { LOG("EgoVehicle autopilot already on"); return; } } - EgoVehiclePtr->SetAutopilot(true); + EgoVehiclePtr.Get()->SetAutopilot(true); LOG("Enabling EgoVehicle Autopilot"); } @@ -445,7 +482,7 @@ void ADReyeVRGameMode::SetupReplayer() Replayer->SetSyncMode(bReplaySync); if (bReplaySync) { - LOG_WARN("Replay operating in frame-wise (1:1) synchronous mode (no replay interpolation)"); + LOG("Replay operating in frame-wise (1:1) synchronous mode (no replay interpolation)"); } bRecorderInitiated = true; } @@ -476,7 +513,7 @@ void ADReyeVRGameMode::DrawBBoxes() BBox->Activate(); BBox->MaterialParams.Opacity = 0.1f; FLinearColor Col = FLinearColor::Green; - if (FVector::Distance(EgoVehiclePtr->GetActorLocation(), A->GetActorLocation()) < DistThresh * 100.f) + if (FVector::Distance(GetEgoVehicle()->GetActorLocation(), A->GetActorLocation()) < DistThresh * 100.f) { Col = FLinearColor::Red; } @@ -563,12 +600,11 @@ void ADReyeVRGameMode::SpawnEgoVehicle(const FTransform &SpawnPt) DReyeVRDescr.UId = EgoVehicleDefn.UId; DReyeVRDescr.Id = EgoVehicleDefn.Id; DReyeVRDescr.Class = EgoVehicleDefn.Class; - // ensure this vehicle is denoted by the 'hero' attribute - FActorAttribute HeroRole; - HeroRole.Id = "role_name"; - HeroRole.Type = EActorAttributeType::String; - HeroRole.Value = "hero"; - DReyeVRDescr.Variations.Add(HeroRole.Id, std::move(HeroRole)); + // add all the attributes from the definition to the description + for (FActorAttribute A : EgoVehicleDefn.Attributes) + { + DReyeVRDescr.Variations.Add(A.Id, std::move(A)); + } } // calls Episode::SpawnActor => SpawnActorWithInfo => ActorDispatcher->SpawnActor => SpawnFunctions[UId] EgoVehiclePtr = static_cast(Episode->SpawnActor(SpawnPt, DReyeVRDescr)); diff --git a/DReyeVR/DReyeVRGameMode.h b/DReyeVR/DReyeVRGameMode.h index 058b5d8..da0a54d 100644 --- a/DReyeVR/DReyeVRGameMode.h +++ b/DReyeVR/DReyeVRGameMode.h @@ -3,12 +3,13 @@ #include "Carla/Actor/DReyeVRCustomActor.h" // ADReyeVRCustomActor #include "Carla/Game/CarlaGameModeBase.h" // ACarlaGameModeBase #include "Carla/Sensor/DReyeVRData.h" // DReyeVR:: +#include "DReyeVRPawn.h" // ADReyeVRPawn +#include "DReyeVRUtils.h" // SafePtrGet #include // std::unordered_map #include "DReyeVRGameMode.generated.h" class AEgoVehicle; -class ADReyeVRPawn; UCLASS() class ADReyeVRGameMode : public ACarlaGameModeBase @@ -24,15 +25,12 @@ class ADReyeVRGameMode : public ACarlaGameModeBase virtual void Tick(float DeltaSeconds) override; - ADReyeVRPawn *GetPawn() - { - return DReyeVR_Pawn; - } + APawn *GetSpectator(); + AEgoVehicle *GetEgoVehicle(); + APlayerController *GetPlayer(); + ADReyeVRPawn *GetPawn(); - void SetEgoVehicle(AEgoVehicle *Vehicle) - { - EgoVehiclePtr = Vehicle; - } + void SetEgoVehicle(AEgoVehicle *Ego); // input handling void SetupPlayerInputComponent(); @@ -71,13 +69,16 @@ class ADReyeVRGameMode : public ACarlaGameModeBase void SetupSpectator(); bool SetupEgoVehicle(); void SpawnEgoVehicle(const FTransform &SpawnPt); - class APlayerController *Player = nullptr; - class ADReyeVRPawn *DReyeVR_Pawn = nullptr; + + // TWeakObjectPtr's allow us to check if the underlying object is alive + // in case it was destroyed by someone other than us (ex. garbage collection) + TWeakObjectPtr Player; + TWeakObjectPtr DReyeVR_Pawn; + TWeakObjectPtr SpectatorPtr; + TWeakObjectPtr EgoVehiclePtr; // for toggling bw spectator mode bool bIsSpectating = true; - class APawn *SpectatorPtr = nullptr; - class AEgoVehicle *EgoVehiclePtr = nullptr; // for audio control float EgoVolumePercent; diff --git a/DReyeVR/DReyeVRPawn.cpp b/DReyeVR/DReyeVRPawn.cpp index 2be078c..77ba8ca 100644 --- a/DReyeVR/DReyeVRPawn.cpp +++ b/DReyeVR/DReyeVRPawn.cpp @@ -1,5 +1,6 @@ #include "DReyeVRPawn.h" #include "DReyeVRUtils.h" // CreatePostProcessingEffect +#include "EgoVehicle.h" // AEgoVehicle #include "HeadMountedDisplayFunctionLibrary.h" // SetTrackingOrigin, GetWorldToMetersScale #include "HeadMountedDisplayTypes.h" // ESpectatorScreenMode #include "Materials/MaterialInstanceDynamic.h" // UMaterialInstanceDynamic diff --git a/DReyeVR/DReyeVRPawn.h b/DReyeVR/DReyeVRPawn.h index 6c8fd51..a0718c4 100644 --- a/DReyeVR/DReyeVRPawn.h +++ b/DReyeVR/DReyeVRPawn.h @@ -1,7 +1,6 @@ #pragma once #include "Camera/CameraComponent.h" // UCameraComponent -#include "EgoVehicle.h" // AEgoVehicle #include "Engine/Scene.h" // FPostProcessSettings #include "GameFramework/Pawn.h" // CreatePlayerInputComponent @@ -17,6 +16,8 @@ #include "DReyeVRPawn.generated.h" +class AEgoVehicle; + UCLASS() class ADReyeVRPawn : public APawn { diff --git a/DReyeVR/DReyeVRUtils.h b/DReyeVR/DReyeVRUtils.h index ba7181b..ecdd218 100644 --- a/DReyeVR/DReyeVRUtils.h +++ b/DReyeVR/DReyeVRUtils.h @@ -13,6 +13,24 @@ #include #include +template +T *SafePtrGet(const FString &Name, TWeakObjectPtr &Ptr, const std::function &RemedyFunction) +{ + if (Ptr.IsValid()) + return Ptr.Get(); + // object was destroyed! possibly by external process (ex. map change) + if (!Ptr.IsExplicitlyNull()) + { // dangling pointer!! + LOG_WARN("Dangling pointer \"%s\" (%p) is invalid! Attempting to remedy", Ptr.Get(), *Name); + } + RemedyFunction(); + // try to remedy + if (Ptr.IsValid()) + return Ptr.Get(); + LOG_ERROR("Unable to remedy (%s)", *Name); + return nullptr; +} + /// this is the file where we'll read all DReyeVR specific configs static const FString ConfigFilePath = FPaths::Combine(FPaths::ConvertRelativePathToFull(FPaths::ProjectDir()), TEXT("Config"), TEXT("DReyeVRConfig.ini")); diff --git a/DReyeVR/EgoSensor.cpp b/DReyeVR/EgoSensor.cpp index 6005ceb..902772a 100644 --- a/DReyeVR/EgoSensor.cpp +++ b/DReyeVR/EgoSensor.cpp @@ -105,11 +105,12 @@ void AEgoSensor::ManualTick(float DeltaSeconds) ComputeEgoVars(); // get all necessary ego-vehicle data // Update the internal sensor data that gets handed off to Carla (for recording/replaying/PythonAPI) - GetData()->Update(Timestamp, // TimestampCarla (ms) - EyeSensorData, // EyeTrackerData - EgoVars, // EgoVehicleVariables - FocusInfoData, // FocusData - Vehicle->GetVehicleInputs() // User inputs + const auto &Inputs = Vehicle.IsValid() ? Vehicle.Get()->GetVehicleInputs() : DReyeVR::UserInputs{}; + GetData()->Update(Timestamp, // TimestampCarla (ms) + EyeSensorData, // EyeTrackerData + EgoVars, // EgoVehicleVariables + FocusInfoData, // FocusData + Inputs // User inputs ); TickFoveatedRender(); } @@ -288,7 +289,8 @@ bool AEgoSensor::ComputeGazeTrace(FHitResult &Hit, const ECollisionChannel Trace // Create collision information container. FCollisionQueryParams TraceParam; TraceParam = FCollisionQueryParams(FName("TraceParam"), true); - TraceParam.AddIgnoredActor(Vehicle); // don't collide with the vehicle since that would be useless + if (Vehicle.IsValid()) + TraceParam.AddIgnoredActor(Vehicle.Get()); // don't collide with the vehicle since that would be useless TraceParam.bTraceComplex = true; TraceParam.bReturnPhysicalMaterial = false; Hit = FHitResult(EForceInit::ForceInit); @@ -371,26 +373,27 @@ float AEgoSensor::ComputeVergence(const FVector &L0, const FVector &LDir, const void AEgoSensor::SetEgoVehicle(class AEgoVehicle *NewEgoVehicle) { Vehicle = NewEgoVehicle; - Camera = Vehicle->GetCamera(); - check(Vehicle); -} - -void AEgoSensor::SetGame(class ADReyeVRGameMode *GameIn) -{ - DReyeVRGame = GameIn; - check(DReyeVRGame); + check(Vehicle.IsValid()); } void AEgoSensor::ComputeEgoVars() { + if (!Vehicle.IsValid()) + { + LOG_WARN("Invalid EgoVehicle, cannot compute EgoVars"); + return; + } // See DReyeVRData::EgoVariables - EgoVars.VehicleLocation = Vehicle->GetActorLocation(); - EgoVars.VehicleRotation = Vehicle->GetActorRotation(); + auto *Ego = Vehicle.Get(); + auto *Camera = Ego->GetCamera(); + check(Camera); + EgoVars.VehicleLocation = Ego->GetActorLocation(); + EgoVars.VehicleRotation = Ego->GetActorRotation(); EgoVars.CameraLocation = Camera->GetRelativeLocation(); EgoVars.CameraRotation = Camera->GetRelativeRotation(); EgoVars.CameraLocationAbs = Camera->GetComponentLocation(); EgoVars.CameraRotationAbs = Camera->GetComponentRotation(); - EgoVars.Velocity = Vehicle->GetVehicleForwardSpeed(); + EgoVars.Velocity = Ego->GetVehicleForwardSpeed(); } /// ========================================== /// @@ -399,7 +402,7 @@ void AEgoSensor::ComputeEgoVars() void AEgoSensor::ConstructFrameCapture() { - if (bCaptureFrameData) + if (bCaptureFrameData && Vehicle.IsValid()) { // Frame capture CaptureRenderTarget = CreateDefaultSubobject(TEXT("CaptureRenderTarget_DReyeVR")); @@ -416,7 +419,7 @@ void AEgoSensor::ConstructFrameCapture() check(CaptureRenderTarget->GetSurfaceWidth() > 0 && CaptureRenderTarget->GetSurfaceHeight() > 0); FrameCap = CreateDefaultSubobject(TEXT("FrameCap")); - FrameCap->SetupAttachment(Camera); + FrameCap->SetupAttachment(Vehicle.Get()->GetCamera()); FrameCap->PrimitiveRenderMode = ESceneCapturePrimitiveRenderMode::PRM_RenderScenePrimitives; FrameCap->bCaptureOnMovement = false; FrameCap->bCaptureEveryFrame = false; @@ -480,25 +483,25 @@ void AEgoSensor::TakeScreenshot() } // capture the screenshot to the directory - if (bCaptureFrameData && FrameCap && Camera && Vehicle) + if (bCaptureFrameData && FrameCap && Vehicle.IsValid()) { for (int i = 0; i < GetNumberOfShaders(); i++) { // apply the postprocessing effect FrameCap->PostProcessSettings = CreatePostProcessingEffect(i); // loop through all camera poses - for (int j = 0; j < Vehicle->GetNumCameraPoses(); j++) + for (int j = 0; j < Vehicle.Get()->GetNumCameraPoses(); j++) { // set this pose - Vehicle->SetCameraRootPose(j); + Vehicle.Get()->SetCameraRootPose(j); // using 5 digits to reach frame 99999 ~ 30m (assuming ~50fps frame capture) // suffix is denoted as _s(hader)X_p(ose)Y_Z.png where X is the shader idx, Y is the pose idx, Z is tick const FString Suffix = FString::Printf(TEXT("_s%d_p%d_%05d.png"), i, j, ScreenshotCount); // apply the camera view (position & orientation) FMinimalViewInfo DesiredView; - Camera->GetCameraView(0, DesiredView); - FrameCap->SetCameraView(DesiredView); // move camera to the Camera view + Vehicle.Get()->GetCamera()->GetCameraView(0, DesiredView); + FrameCap->SetCameraView(DesiredView); // move camera to the camera view // capture the scene and save the screenshot to disk FrameCap->CaptureScene(); // also available: CaptureSceneDeferred() SaveFrameToDisk(*CaptureRenderTarget, FPaths::Combine(FrameCapLocation, FrameCapFilename + Suffix), @@ -510,7 +513,7 @@ void AEgoSensor::TakeScreenshot() } } // set camera pose back to 0 - Vehicle->SetCameraRootPose(0); + Vehicle.Get()->SetCameraRootPose(0); if (!bRecordAllShaders) { // exit after the first shader (rgb) @@ -553,14 +556,8 @@ void AEgoSensor::TickFoveatedRender() /// ----------------:REPLAY:------------------ /// /// ========================================== /// -void AEgoSensor::UpdateData(const DReyeVR::AggregateData &RecorderData, const double Per) -{ - // call the parent function - ADReyeVRSensor::UpdateData(RecorderData, Per); -} - void AEgoSensor::UpdateData(const DReyeVR::CustomActorData &RecorderData, const double Per) { - if (DReyeVRGame) - DReyeVRGame->ReplayCustomActor(RecorderData, Per); + if (Vehicle.IsValid() && Vehicle.Get()->GetGame()) + Vehicle.Get()->GetGame()->ReplayCustomActor(RecorderData, Per); } \ No newline at end of file diff --git a/DReyeVR/EgoSensor.h b/DReyeVR/EgoSensor.h index 31efebf..c0387b0 100644 --- a/DReyeVR/EgoSensor.h +++ b/DReyeVR/EgoSensor.h @@ -37,9 +37,7 @@ class CARLAUE4_API AEgoSensor : public ADReyeVRSensor void ManualTick(float DeltaSeconds); // Tick called explicitly from DReyeVR EgoVehicle void SetEgoVehicle(class AEgoVehicle *EgoVehicle); // provide access to EgoVehicle (and by extension its camera) - void SetGame(class ADReyeVRGameMode *Game); // provides access to ADReyeVRGameMode - void UpdateData(const DReyeVR::AggregateData &RecorderData, const double Per) override; void UpdateData(const DReyeVR::CustomActorData &RecorderData, const double Per) override; // function where replayer requests a screenshot @@ -56,7 +54,7 @@ class CARLAUE4_API AEgoSensor : public ADReyeVRSensor int64_t TickCount = 0; // how many ticks have been executed void ReadConfigVariables(); - ////////////////:EYETRACKER://////////////// + private: // eye tracker void InitEyeTracker(); void DestroyEyeTracker(); void ComputeDummyEyeData(); // when no hardware sensor is present @@ -76,16 +74,15 @@ class CARLAUE4_API AEgoSensor : public ADReyeVRSensor struct DReyeVR::FocusInfo FocusInfoData; // data from the focus computed from eye gaze std::chrono::time_point ChronoStartTime; // std::chrono time at BeginPlay - ////////////////:EGOVARS://////////////// + private: // ego=vehicle variables void ComputeEgoVars(); - class AEgoVehicle *Vehicle; // the DReyeVR EgoVehicle - struct DReyeVR::EgoVariables EgoVars; // data from vehicle that is getting tracked + TWeakObjectPtr Vehicle; // the DReyeVR EgoVehicle + struct DReyeVR::EgoVariables EgoVars; // data from vehicle that is getting tracked - ////////////////:FRAMECAPTURE://////////////// + private: // frame capture + size_t ScreenshotCount = 0; void ConstructFrameCapture(); // needs to be called in the constructor void InitFrameCapture(); // needs to be called in BeginPlay - size_t ScreenshotCount = 0; - class UCameraComponent *Camera; // for frame capture views class UTextureRenderTarget2D *CaptureRenderTarget = nullptr; class USceneCaptureComponent2D *FrameCap = nullptr; FString FrameCapLocation; // relative to game dir @@ -99,12 +96,9 @@ class CARLAUE4_API AEgoSensor : public ADReyeVRSensor bool bFileFormatJPG = true; bool bFrameCapForceLinearGamma = true; - ////////////////:FOVEATEDRENDER://////////////// + private: // foveated rendering void TickFoveatedRender(); void ConvertToEyeTrackerSpace(FVector &inVec) const; bool bEnableFovRender = false; bool bUseEyeTrackingVRS = true; - - ////////////////:REPLAY://////////////// - class ADReyeVRGameMode *DReyeVRGame = nullptr; }; \ No newline at end of file diff --git a/DReyeVR/EgoVehicle.cpp b/DReyeVR/EgoVehicle.cpp index 3cf2b83..30aa832 100644 --- a/DReyeVR/EgoVehicle.cpp +++ b/DReyeVR/EgoVehicle.cpp @@ -110,12 +110,10 @@ void AEgoVehicle::EndPlay(const EEndPlayReason::Type EndPlayReason) if (EndPlayReason == EEndPlayReason::Destroyed) { LOG("DReyeVR EgoVehicle is being destroyed! You'll need to spawn another one!"); - } - - if (GetGame()) - { - GetGame()->SetEgoVehicle(nullptr); - GetGame()->PossessSpectator(); + if (GetGame()) + { + GetGame()->PossessSpectator(); + } } if (this->Pawn) @@ -129,8 +127,8 @@ void AEgoVehicle::BeginDestroy() Super::BeginDestroy(); // destroy all spawned entities - if (EgoSensor) - EgoSensor->Destroy(); + if (EgoSensor.IsValid()) + EgoSensor.Get()->Destroy(); LOG("EgoVehicle has been destroyed"); } @@ -358,9 +356,16 @@ FRotator AEgoVehicle::GetCameraRot() const { return GetCamera() ? GetCamera()->GetComponentRotation() : FRotator::ZeroRotator; } +class AEgoSensor *AEgoVehicle::GetSensor() +{ + // tries to return the EgoSensor pointer if it is safe to do so, else re-spawn it + return SafePtrGet("EgoSensor", EgoSensor, [&](void) { this->InitSensor(); }); +} + const class AEgoSensor *AEgoVehicle::GetSensor() const { - return const_cast(EgoSensor); + // tries to return the EgoSensor pointer if it is safe to do so, else re-spawn it + return const_cast(const_cast(this)->GetSensor()); } /// ========================================== /// @@ -416,28 +421,29 @@ void AEgoVehicle::InitSensor() Description.UId = EgoSensorDef.UId; Description.Id = EgoSensorDef.Id; Description.Class = EgoSensorDef.Class; - } - - if (Episode == nullptr) - { - LOG_ERROR("Null Episode in world!"); + // add all the attributes from the definition to the description + for (FActorAttribute A : EgoSensorDef.Attributes) + { + Description.Variations.Add(A.Id, std::move(A)); + } } // calls Episode::SpawnActor => SpawnActorWithInfo => ActorDispatcher->SpawnActor => SpawnFunctions[UId] FTransform SpawnPt = FTransform(FRotator::ZeroRotator, GetCameraPosn(), FVector::OneVector); EgoSensor = static_cast(Episode->SpawnActor(SpawnPt, Description)); } - check(EgoSensor != nullptr); + check(EgoSensor.IsValid()); // Attach the EgoSensor as a child to the EgoVehicle - EgoSensor->SetOwner(this); - EgoSensor->AttachToActor(this, FAttachmentTransformRules::KeepRelativeTransform); - EgoSensor->SetEgoVehicle(this); - if (DReyeVRGame) - EgoSensor->SetGame(DReyeVRGame); + EgoSensor.Get()->SetOwner(this); + EgoSensor.Get()->AttachToActor(this, FAttachmentTransformRules::KeepRelativeTransform); + EgoSensor.Get()->SetActorTransform(FTransform::Identity, false, nullptr, ETeleportType::TeleportPhysics); + EgoSensor.Get()->SetEgoVehicle(this); } void AEgoVehicle::ReplayTick() { - const bool bIsReplaying = EgoSensor->IsReplaying(); + if (!EgoSensor.IsValid()) + return; + const bool bIsReplaying = EgoSensor.Get()->IsReplaying(); // need to enable/disable VehicleMesh simulation class USkeletalMeshComponent *VehicleMesh = GetMesh(); if (VehicleMesh) @@ -449,7 +455,7 @@ void AEgoVehicle::ReplayTick() if (bIsReplaying) { // this gets reached when the simulator is replaying data from a carla log - const DReyeVR::AggregateData *Replay = EgoSensor->GetData(); + const DReyeVR::AggregateData *Replay = EgoSensor.Get()->GetData(); // include positional update here, else there is lag/jitter between the camera and the vehicle // since the Carla Replayer tick differs from the EgoVehicle tick @@ -475,22 +481,21 @@ void AEgoVehicle::ReplayTick() void AEgoVehicle::UpdateSensor(const float DeltaSeconds) { - if (EgoSensor == nullptr) // Spawn and attach the EgoSensor + if (!EgoSensor.IsValid()) // Spawn and attach the EgoSensor { // unfortunately World->SpawnActor *sometimes* fails if used in BeginPlay so // calling it once in the tick is fine to avoid this crash. InitSensor(); } - ensure(EgoSensor != nullptr); - if (EgoSensor == nullptr) + if (!EgoSensor.IsValid()) { LOG_WARN("EgoSensor initialization failed!"); return; } // Explicitly update the EgoSensor here, synchronized with EgoVehicle tick - EgoSensor->ManualTick(DeltaSeconds); // Ensures we always get the latest data + EgoSensor.Get()->ManualTick(DeltaSeconds); // Ensures we always get the latest data } /// ========================================== /// @@ -676,9 +681,9 @@ void AEgoVehicle::UpdateDash() { // Draw text components float XPH; // miles-per-hour or km-per-hour - if (EgoSensor->IsReplaying()) + if (EgoSensor.IsValid() && EgoSensor.Get()->IsReplaying()) { - const DReyeVR::AggregateData *Replay = EgoSensor->GetData(); + const DReyeVR::AggregateData *Replay = EgoSensor.Get()->GetData(); XPH = Replay->GetVehicleVelocity() * SpeedometerScale; // FwdSpeed is in cm/s const auto &ReplayInputs = Replay->GetUserInputs(); if (ReplayInputs.ToggledReverse) @@ -782,7 +787,7 @@ void AEgoVehicle::SetGame(ADReyeVRGameMode *Game) { DReyeVRGame = Game; check(DReyeVRGame != nullptr); - DReyeVRGame->SetEgoVehicle(this); + check(DReyeVRGame->GetEgoVehicle() == this); DReyeVRGame->GetPawn()->BeginEgoVehicle(this, World); LOG("Successfully assigned GameMode & controller pawn"); @@ -807,10 +812,10 @@ void AEgoVehicle::DebugLines() const { #if WITH_EDITOR - if (bDrawDebugEditor) + if (bDrawDebugEditor && EgoSensor.IsValid()) { // Calculate gaze data (in world space) using eye tracker data - const DReyeVR::AggregateData *Data = EgoSensor->GetData(); + const DReyeVR::AggregateData *Data = EgoSensor.Get()->GetData(); // Compute World positions and orientations const FRotator &WorldRot = FirstPersonCam->GetComponentRotation(); const FVector &WorldPos = FirstPersonCam->GetComponentLocation(); diff --git a/DReyeVR/EgoVehicle.h b/DReyeVR/EgoVehicle.h index bf56ad2..9343f0c 100644 --- a/DReyeVR/EgoVehicle.h +++ b/DReyeVR/EgoVehicle.h @@ -98,9 +98,11 @@ class CARLAUE4_API AEgoVehicle : public ACarlaWheeledVehicle bool bCameraFollowHMD = true; // disable this (in params) to replay without following the player's HMD (replay-only) ////////////////:SENSOR://////////////// + class AEgoSensor *GetSensor(); void ReplayTick(); void InitSensor(); - class AEgoSensor *EgoSensor; // custom sensor helper that holds logic for extracting useful data + // custom sensor helper that holds logic for extracting useful data + TWeakObjectPtr EgoSensor; // EgoVehicle should have exclusive access (TODO: unique ptr?) void UpdateSensor(const float DeltaTime); ///////////////:DREYEVRPAWN:///////////// diff --git a/LibCarla/source/carla/trafficmanager/ALSM.cpp b/LibCarla/source/carla/trafficmanager/ALSM.cpp new file mode 100644 index 0000000..eee90c2 --- /dev/null +++ b/LibCarla/source/carla/trafficmanager/ALSM.cpp @@ -0,0 +1,403 @@ + +#include "boost/pointer_cast.hpp" + +#include "carla/client/Actor.h" +#include "carla/client/Vehicle.h" +#include "carla/client/Walker.h" + +#include "carla/trafficmanager/Constants.h" +#include "carla/trafficmanager/LocalizationUtils.h" +#include "carla/trafficmanager/SimpleWaypoint.h" + +#include "carla/trafficmanager/ALSM.h" + +namespace carla { +namespace traffic_manager { + +ALSM::ALSM( + AtomicActorSet ®istered_vehicles, + BufferMap &buffer_map, + TrackTraffic &track_traffic, + std::vector& marked_for_removal, + const Parameters ¶meters, + const cc::World &world, + const LocalMapPtr &local_map, + SimulationState &simulation_state, + LocalizationStage &localization_stage, + CollisionStage &collision_stage, + TrafficLightStage &traffic_light_stage, + MotionPlanStage &motion_plan_stage, + VehicleLightStage &vehicle_light_stage, + RandomGeneratorMap &random_devices) + : registered_vehicles(registered_vehicles), + buffer_map(buffer_map), + track_traffic(track_traffic), + marked_for_removal(marked_for_removal), + parameters(parameters), + world(world), + local_map(local_map), + simulation_state(simulation_state), + localization_stage(localization_stage), + collision_stage(collision_stage), + traffic_light_stage(traffic_light_stage), + motion_plan_stage(motion_plan_stage), + vehicle_light_stage(vehicle_light_stage), + random_devices(random_devices) {} + +void ALSM::Update() { + + bool hybrid_physics_mode = parameters.GetHybridPhysicsMode(); + + std::set world_vehicle_ids; + std::set world_pedestrian_ids; + std::vector unregistered_list_to_be_deleted; + + current_timestamp = world.GetSnapshot().GetTimestamp(); + ActorList world_actors = world.GetActors(); + + // Find destroyed actors and perform clean up. + const ALSM::DestroyeddActors destroyed_actors = IdentifyDestroyedActors(world_actors); + + const ActorIdSet &destroyed_registered = destroyed_actors.first; + for (const auto &deletion_id: destroyed_registered) { + RemoveActor(deletion_id, true); + } + + const ActorIdSet &destroyed_unregistered = destroyed_actors.second; + for (auto deletion_id : destroyed_unregistered) { + RemoveActor(deletion_id, false); + } + + // Invalidate hero actor if it is not alive anymore. + if (hero_actors.size() != 0u) { + ActorIdSet hero_actors_to_delete; + for (auto &hero_actor_info: hero_actors) { + if (destroyed_unregistered.find(hero_actor_info.first) != destroyed_unregistered.end()) { + hero_actors_to_delete.insert(hero_actor_info.first); + } + if (destroyed_registered.find(hero_actor_info.first) != destroyed_registered.end()) { + hero_actors_to_delete.insert(hero_actor_info.first); + } + } + + for (auto &deletion_id: hero_actors_to_delete) { + hero_actors.erase(deletion_id); + } + } + + // Scan for new unregistered actors. + IdentifyNewActors(world_actors); + + // Update dynamic state and static attributes for all registered vehicles. + ALSM::IdleInfo max_idle_time = std::make_pair(0u, current_timestamp.elapsed_seconds); + UpdateRegisteredActorsData(hybrid_physics_mode, max_idle_time); + + // Destroy registered vehicle if stuck at a location for too long. + if (IsVehicleStuck(max_idle_time.first) + && (current_timestamp.elapsed_seconds - elapsed_last_actor_destruction) > DELTA_TIME_BETWEEN_DESTRUCTIONS + && hero_actors.find(max_idle_time.first) == hero_actors.end()) { + registered_vehicles.Destroy(max_idle_time.first); + RemoveActor(max_idle_time.first, true); + elapsed_last_actor_destruction = current_timestamp.elapsed_seconds; + } + + // Destorying vehicles for marked for removal by stages. + if (parameters.GetOSMMode()) { + for (const ActorId& actor_id: marked_for_removal) { + registered_vehicles.Destroy(actor_id); + RemoveActor(actor_id, true); + } + marked_for_removal.clear(); + } + + // Update dynamic state and static attributes for unregistered actors. + UpdateUnregisteredActorsData(); +} + +void ALSM::IdentifyNewActors(const ActorList &actor_list) { + for (auto iter = actor_list->begin(); iter != actor_list->end(); ++iter) { + ActorPtr actor = *iter; + ActorId actor_id = actor->GetId(); + // Identify any new hero vehicle + if (actor->GetTypeId().front() == 'v' || actor->GetTypeId().rfind("harplab.dreyevr_vehicle.") == 0) { + if (hero_actors.size() == 0u || hero_actors.find(actor_id) == hero_actors.end()) { + for (auto&& attribute: actor->GetAttributes()) { + if (attribute.GetId() == "role_name" && attribute.GetValue() == "hero") { + hero_actors.insert({actor_id, actor}); + } + } + } + } + if (!registered_vehicles.Contains(actor_id) + && unregistered_actors.find(actor_id) == unregistered_actors.end()) { + + unregistered_actors.insert({actor_id, actor}); + } + } +} + +ALSM::DestroyeddActors ALSM::IdentifyDestroyedActors(const ActorList &actor_list) { + + ALSM::DestroyeddActors destroyed_actors; + ActorIdSet &deleted_registered = destroyed_actors.first; + ActorIdSet &deleted_unregistered = destroyed_actors.second; + + // Building hash set of actors present in current frame. + ActorIdSet current_actors; + for (auto iter = actor_list->begin(); iter != actor_list->end(); ++iter) { + current_actors.insert((*iter)->GetId()); + } + + // Searching for destroyed registered actors. + std::vector registered_ids = registered_vehicles.GetIDList(); + for (const ActorId &actor_id : registered_ids) { + if (current_actors.find(actor_id) == current_actors.end()) { + deleted_registered.insert(actor_id); + } + } + + // Searching for destroyed unregistered actors. + for (const auto &actor_info: unregistered_actors) { + const ActorId &actor_id = actor_info.first; + if (current_actors.find(actor_id) == current_actors.end() + || registered_vehicles.Contains(actor_id)) { + deleted_unregistered.insert(actor_id); + } + } + + return destroyed_actors; +} + +void ALSM::UpdateRegisteredActorsData(const bool hybrid_physics_mode, ALSM::IdleInfo &max_idle_time) { + + std::vector vehicle_list = registered_vehicles.GetList(); + bool hero_actor_present = hero_actors.size() != 0u; + float physics_radius = parameters.GetHybridPhysicsRadius(); + float physics_radius_square = SQUARE(physics_radius); + bool is_respawn_vehicles = parameters.GetRespawnDormantVehicles(); + if (is_respawn_vehicles && !hero_actor_present) { + track_traffic.SetHeroLocation(cg::Location(0,0,0)); + } + // Update first the information regarding any hero vehicle. + for (auto &hero_actor_info: hero_actors){ + if (is_respawn_vehicles) { + track_traffic.SetHeroLocation(hero_actor_info.second->GetTransform().location); + } + UpdateData(hybrid_physics_mode, max_idle_time, hero_actor_info.second, hero_actor_present, physics_radius_square); + } + // Update information for all other registered vehicles. + for (const Actor &vehicle : vehicle_list) { + ActorId actor_id = vehicle->GetId(); + if (hero_actors.find(actor_id) == hero_actors.end()) { + UpdateData(hybrid_physics_mode, max_idle_time, vehicle, hero_actor_present, physics_radius_square); + } + } +} + +void ALSM::UpdateData(const bool hybrid_physics_mode, + ALSM::IdleInfo &max_idle_time, const Actor &vehicle, + const bool hero_actor_present, const float physics_radius_square) { + + ActorId actor_id = vehicle->GetId(); + cg::Transform vehicle_transform = vehicle->GetTransform(); + cg::Location vehicle_location = vehicle_transform.location; + cg::Rotation vehicle_rotation = vehicle_transform.rotation; + cg::Vector3D vehicle_velocity = vehicle->GetVelocity(); + + // Initializing idle times. + if (idle_time.find(actor_id) == idle_time.end() && current_timestamp.elapsed_seconds != 0.0) { + idle_time.insert({actor_id, current_timestamp.elapsed_seconds}); + } + + // Check if current actor is in range of hero actor and enable physics in hybrid mode. + bool in_range_of_hero_actor = false; + if (hero_actor_present && hybrid_physics_mode) { + for (auto &hero_actor_info: hero_actors) { + const ActorId &hero_actor_id = hero_actor_info.first; + if (simulation_state.ContainsActor(hero_actor_id)) { + const cg::Location &hero_location = simulation_state.GetLocation(hero_actor_id); + if (cg::Math::DistanceSquared(vehicle_location, hero_location) < physics_radius_square) { + in_range_of_hero_actor = true; + break; + } + } + } + } + + bool enable_physics = hybrid_physics_mode ? in_range_of_hero_actor : true; + if (!has_physics_enabled.count(actor_id) || has_physics_enabled[actor_id] != enable_physics) { + if (hero_actors.find(actor_id) == hero_actors.end()) { + vehicle->SetSimulatePhysics(enable_physics); + has_physics_enabled[actor_id] = enable_physics; + if (enable_physics == true && simulation_state.ContainsActor(actor_id)) { + vehicle->SetTargetVelocity(simulation_state.GetVelocity(actor_id)); + } + } + } + + bool state_entry_present = simulation_state.ContainsActor(actor_id); + // If physics is disabled, calculate velocity based on change in position. + if (!enable_physics) { + cg::Location previous_location; + if (state_entry_present) { + previous_location = simulation_state.GetLocation(actor_id); + } else { + previous_location = vehicle_location; + } + cg::Vector3D displacement = (vehicle_location - previous_location); + vehicle_velocity = displacement * INV_HYBRID_DT; + } + + // Updated kinematic state object. + auto vehicle_ptr = boost::static_pointer_cast(vehicle); + KinematicState kinematic_state{vehicle_location, vehicle_rotation, + vehicle_velocity, vehicle_ptr->GetSpeedLimit(), + enable_physics, vehicle->IsDormant()}; + + // Updated traffic light state object. + TrafficLightState tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()}; + + // Update simulation state. + if (state_entry_present) { + simulation_state.UpdateKinematicState(actor_id, kinematic_state); + simulation_state.UpdateTrafficLightState(actor_id, tl_state); + } + else { + cg::Vector3D dimensions = vehicle_ptr->GetBoundingBox().extent; + StaticAttributes attributes{ActorType::Vehicle, dimensions.x, dimensions.y, dimensions.z}; + + simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state); + } + + // Updating idle time when necessary. + UpdateIdleTime(max_idle_time, actor_id); +} + + +void ALSM::UpdateUnregisteredActorsData() { + for (auto &actor_info: unregistered_actors) { + + const ActorId actor_id = actor_info.first; + const ActorPtr actor_ptr = actor_info.second; + const std::string type_id = actor_ptr->GetTypeId(); + + const cg::Transform actor_transform = actor_ptr->GetTransform(); + const cg::Location actor_location = actor_transform.location; + const cg::Rotation actor_rotation = actor_transform.rotation; + const cg::Vector3D actor_velocity = actor_ptr->GetVelocity(); + const bool actor_is_dormant = actor_ptr->IsDormant(); + KinematicState kinematic_state {actor_location, actor_rotation, actor_velocity, -1.0f, true, actor_is_dormant}; + + TrafficLightState tl_state; + ActorType actor_type = ActorType::Any; + cg::Vector3D dimensions; + std::vector nearest_waypoints; + + bool state_entry_not_present = !simulation_state.ContainsActor(actor_id); + if (type_id.front() == 'v' || type_id.rfind("harplab.dreyevr_vehicle.") == 0) { // include DReyeVR vehicle + auto vehicle_ptr = boost::static_pointer_cast(actor_ptr); + kinematic_state.speed_limit = vehicle_ptr->GetSpeedLimit(); + + tl_state = {vehicle_ptr->GetTrafficLightState(), vehicle_ptr->IsAtTrafficLight()}; + + if (state_entry_not_present) { + dimensions = vehicle_ptr->GetBoundingBox().extent; + actor_type = ActorType::Vehicle; + StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z}; + + simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state); + } else { + simulation_state.UpdateKinematicState(actor_id, kinematic_state); + simulation_state.UpdateTrafficLightState(actor_id, tl_state); + } + + // Identify occupied waypoints. + cg::Vector3D extent = vehicle_ptr->GetBoundingBox().extent; + cg::Vector3D heading_vector = vehicle_ptr->GetTransform().GetForwardVector(); + std::vector corners = {actor_location + cg::Location(extent.x * heading_vector), + actor_location, + actor_location + cg::Location(-extent.x * heading_vector)}; + for (cg::Location &vertex: corners) { + SimpleWaypointPtr nearest_waypoint = local_map->GetWaypoint(vertex); + nearest_waypoints.push_back(nearest_waypoint); + } + } + else if (type_id.front() == 'w') { + auto walker_ptr = boost::static_pointer_cast(actor_ptr); + + if (state_entry_not_present) { + dimensions = walker_ptr->GetBoundingBox().extent; + actor_type = ActorType::Pedestrian; + StaticAttributes attributes {actor_type, dimensions.x, dimensions.y, dimensions.z}; + + simulation_state.AddActor(actor_id, kinematic_state, attributes, tl_state); + } else { + simulation_state.UpdateKinematicState(actor_id, kinematic_state); + } + + // Identify occupied waypoints. + SimpleWaypointPtr nearest_waypoint = local_map->GetWaypoint(actor_location); + nearest_waypoints.push_back(nearest_waypoint); + } + + track_traffic.UpdateUnregisteredGridPosition(actor_id, nearest_waypoints); + } +} + +void ALSM::UpdateIdleTime(std::pair& max_idle_time, const ActorId& actor_id) { + if (idle_time.find(actor_id) != idle_time.end()) { + double &idle_duration = idle_time.at(actor_id); + if (simulation_state.GetVelocity(actor_id).SquaredLength() > SQUARE(STOPPED_VELOCITY_THRESHOLD)) { + idle_duration = current_timestamp.elapsed_seconds; + } + + // Checking maximum idle time. + if (max_idle_time.first == 0u || max_idle_time.second > idle_duration) { + max_idle_time = std::make_pair(actor_id, idle_duration); + } + } +} + +bool ALSM::IsVehicleStuck(const ActorId& actor_id) { + if (idle_time.find(actor_id) != idle_time.end()) { + double delta_idle_time = current_timestamp.elapsed_seconds - idle_time.at(actor_id); + TrafficLightState tl_state = simulation_state.GetTLS(actor_id); + if ((!tl_state.at_traffic_light && tl_state.tl_state != TLS::Red && delta_idle_time >= BLOCKED_TIME_THRESHOLD) + || (delta_idle_time >= RED_TL_BLOCKED_TIME_THRESHOLD)) { + return true; + } + } + return false; +} + +void ALSM::RemoveActor(const ActorId actor_id, const bool registered_actor) { + if (registered_actor) { + registered_vehicles.Remove({actor_id}); + buffer_map.erase(actor_id); + idle_time.erase(actor_id); + random_devices.erase(actor_id); + localization_stage.RemoveActor(actor_id); + collision_stage.RemoveActor(actor_id); + traffic_light_stage.RemoveActor(actor_id); + motion_plan_stage.RemoveActor(actor_id); + vehicle_light_stage.RemoveActor(actor_id); + } + else { + unregistered_actors.erase(actor_id); + hero_actors.erase(actor_id); + } + + track_traffic.DeleteActor(actor_id); + simulation_state.RemoveActor(actor_id); +} + +void ALSM::Reset() { + unregistered_actors.clear(); + idle_time.clear(); + hero_actors.clear(); + elapsed_last_actor_destruction = 0.0; + current_timestamp = world.GetSnapshot().GetTimestamp(); +} + +} // namespace traffic_manager +} // namespace carla