CARLA
LargeMapManager.cpp
Go to the documentation of this file.
1 // Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB).
2 // This work is licensed under the terms of the MIT license.
3 // For a copy, see <https://opensource.org/licenses/MIT>.
4 
5 #include "LargeMapManager.h"
6 
7 #include "Engine/WorldComposition.h"
8 #include "Engine/EngineTypes.h"
9 #include "Components/PrimitiveComponent.h"
10 #include "Landscape.h"
11 #include "LandscapeHeightfieldCollisionComponent.h"
12 #include "LandscapeComponent.h"
13 
15 
16 #include "Walker/WalkerBase.h"
17 #include "Carla/Game/Tagger.h"
19 
20 #include "FileHelper.h"
21 #include "Paths.h"
22 
23 #define LARGEMAP_LOGS 1
24 
25 #if LARGEMAP_LOGS
26 #define LM_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
27 #else
28 #define LM_LOG(...)
29 #endif
30 
31 // Sets default values
33 {
34  PrimaryActorTick.bCanEverTick = true;
35  // PrimaryActorTick.TickInterval = TickInterval;
36 }
37 
39 {
40  /// Remove delegates
41  // Origin rebase
42  FCoreDelegates::PreWorldOriginOffset.RemoveAll(this);
43  FCoreDelegates::PostWorldOriginOffset.RemoveAll(this);
44  // Level added/removed from world
45  FWorldDelegates::LevelRemovedFromWorld.RemoveAll(this);
46  FWorldDelegates::LevelAddedToWorld.RemoveAll(this);
47 
48 }
49 
50 // Called when the game starts or when spawned
52 {
53  Super::BeginPlay();
55 
56  UWorld* World = GetWorld();
57  /// Setup delegates
58  // Origin rebase
59  FCoreDelegates::PreWorldOriginOffset.AddUObject(this, &ALargeMapManager::PreWorldOriginOffset);
60  FCoreDelegates::PostWorldOriginOffset.AddUObject(this, &ALargeMapManager::PostWorldOriginOffset);
61  // Level added/removed from world
62  FWorldDelegates::LevelAddedToWorld.AddUObject(this, &ALargeMapManager::OnLevelAddedToWorld);
63  FWorldDelegates::LevelRemovedFromWorld.AddUObject(this, &ALargeMapManager::OnLevelRemovedFromWorld);
64 
65  // Setup Origin rebase settings
66  UWorldComposition* WorldComposition = World->WorldComposition;
67  WorldComposition->bRebaseOriginIn3DSpace = true;
68  WorldComposition->RebaseOriginDistance = RebaseOriginDistance;
69 
73 
74  // Look for terramechanics actor
75  TArray<AActor*> FoundActors;
76  UGameplayStatics::GetAllActorsOfClass(GetWorld(), AActor::StaticClass(), FoundActors);
77  for(auto CurrentActor : FoundActors)
78  {
79  if( CurrentActor->FindComponentByClass( UCustomTerrainPhysicsComponent::StaticClass() ) != nullptr )
80  {
81  bHasTerramechanics = true;
82  break;
83  }
84  }
85 
86  // Get spectator
87  APlayerController* PlayerController = UGameplayStatics::GetPlayerController(GetWorld(), 0);
88  if(PlayerController)
89  {
90  Spectator = PlayerController->GetPawnOrSpectator();
91  }
92 }
93 
94 void ALargeMapManager::PreWorldOriginOffset(UWorld* InWorld, FIntVector InSrcOrigin, FIntVector InDstOrigin)
95 {
96  LM_LOG(Log, "PreWorldOriginOffset Src: %s -> Dst: %s", *InSrcOrigin.ToString(), *InDstOrigin.ToString());
97 }
98 
99 void ALargeMapManager::PostWorldOriginOffset(UWorld* InWorld, FIntVector InSrcOrigin, FIntVector InDstOrigin)
100 {
101  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::PostWorldOriginOffset);
102  CurrentOriginInt = InDstOrigin;
103  CurrentOriginD = FDVector(InDstOrigin);
104 
105  UWorld* World = GetWorld();
106  UCarlaEpisode* CarlaEpisode = UCarlaStatics::GetCurrentEpisode(World);
107  CarlaEpisode->SetCurrentMapOrigin(CurrentOriginInt);
108 
109 #if WITH_EDITOR
110  GEngine->AddOnScreenDebugMessage(66, MsgTime, FColor::Yellow,
111  FString::Printf(TEXT("Src: %s -> Dst: %s"), *InSrcOrigin.ToString(), *InDstOrigin.ToString()));
112  LM_LOG(Log, "PostWorldOriginOffset Src: %s -> Dst: %s", *InSrcOrigin.ToString(), *InDstOrigin.ToString());
113 
114  // This is just to update the color of the msg with the same as the closest map
115  const TArray<ULevelStreaming*>& StreamingLevels = World->GetStreamingLevels();
116  FColor LevelColor = FColor::White;
117  float MinDistance = 10000000.0f;
118  for (const auto& TilePair : MapTiles)
119  {
120  const FCarlaMapTile& Tile = TilePair.Value;
121  const ULevelStreaming* Level = Tile.StreamingLevel;
122  FVector LevelLocation = Tile.Location;
123  float Distance = FVector::Dist(LevelLocation, FVector(InDstOrigin));
124  if (Distance < MinDistance)
125  {
126  MinDistance = Distance;
127  PositonMsgColor = Level->LevelColor.ToFColor(false);
128  }
129  }
130 #endif // WITH_EDITOR
131 }
132 
133 void ALargeMapManager::OnLevelAddedToWorld(ULevel* InLevel, UWorld* InWorld)
134 {
135  LM_LOG(Warning, "OnLevelAddedToWorld");
136  ATagger::TagActorsInLevel(*InLevel, true);
137 
138 
139  //FDebug::DumpStackTraceToLog(ELogVerbosity::Log);
140 }
141 
142 void ALargeMapManager::OnLevelRemovedFromWorld(ULevel* InLevel, UWorld* InWorld)
143 {
144  LM_LOG(Warning, "OnLevelRemovedFromWorld");
145  //FDebug::DumpStackTraceToLog(ELogVerbosity::Log);
146  FCarlaMapTile& Tile = GetCarlaMapTile(InLevel);
147  Tile.TilesSpawned = false;
148 }
149 
151 {
152  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::RegisterInitialObjects);
153  UWorld* World = GetWorld();
154  UCarlaEpisode* CurrentEpisode = UCarlaStatics::GetCurrentEpisode(World);
155  const FActorRegistry& ActorRegistry = CurrentEpisode->GetActorRegistry();
156  for (const auto& CarlaActorPair : ActorRegistry)
157  {
158  if (CarlaActorPair.Value->GetActorInfo()->Description.Id == "spectator")
159  {
160  continue;
161  }
162  OnActorSpawned(*CarlaActorPair.Value.Get());
163  }
164 }
165 
167  const FCarlaActor& CarlaActor)
168 {
169  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::OnActorSpawned);
170  UWorld* World = GetWorld();
171  const FActorInfo* ActorInfo = CarlaActor.GetActorInfo();
172  AActor* Actor = const_cast<AActor*>(CarlaActor.GetActor());
173  bool IsHeroVehicle = false;
174 
175  // LM_LOG(Warning, "ALargeMapManager::OnActorSpawned func %s %s", *Actor->GetName(), *Actor->GetTranslation().ToString());
176 
177  if (Actor && CarlaActor.GetActorType() == FCarlaActor::ActorType::Vehicle)
178  { // Check if is hero vehicle
179 
180  assert(ActorInfo);
181 
182  const FActorDescription& Description = ActorInfo->Description;
183  const FActorAttribute* Attribute = Description.Variations.Find("role_name");
184  // If is the hero vehicle
185  if(Attribute && (Attribute->Value.Contains("hero") || Attribute->Value.Contains("ego_vehicle")))
186  {
187  LM_LOG(Log, "HERO VEHICLE DETECTED");
188 
189  if (ActorsToConsider.Num() == 1 && ActorsToConsider.Contains(Spectator))
190  {
191  ActorsToConsider.Reset();
192  }
193  ActorsToConsider.Add(Actor);
194 
196 
198 
199  // Wait until the pending levels changes are finished to avoid spawning
200  // the car without ground underneath
201  World->FlushLevelStreaming();
202 
203  IsHeroVehicle = true;
204  }
205  }
206 
207  // Any other actor that its role is not "hero"
208  if(!IsHeroVehicle)
209  {
210  UCarlaEpisode* CurrentEpisode = UCarlaStatics::GetCurrentEpisode(World);
211  const FActorRegistry& ActorRegistry = CurrentEpisode->GetActorRegistry();
212 
213  // Any actor that is not the hero vehicle could possible be destroyed at some point
214  // we need to store the CarlaActor information to be able to spawn it again if needed
215 
216  if(IsValid(Actor))
217  { // Actor was spwaned succesfully
218  // TODO: not dormant but not hero => ActiveActor
219  // LM: Map<AActor* FActiveActor> maybe per tile and in a tile sublevel?
220 
221  LM_LOG(Log, "ACTIVE VEHICLE DETECTED");
222  ActiveActors.Add(CarlaActor.GetActorId());
223  }
224  else
225  { // Actor was spawned as dormant
226  // TODO: dormant => no actor so Actorview stored per tile
227  // LM: Map<ActorId, TileID> , Tile: Map<ActorID, FDormantActor>
228  // In case of update: update Tile Map, update LM Map
229  LM_LOG(Log, "DORMANT VEHICLE DETECTED");
230  DormantActors.Add(CarlaActor.GetActorId());
231  }
232  }
233 
234  if (IsValid(Actor)) {
235  Actor->OnDestroyed.AddDynamic(this, &ALargeMapManager::OnActorDestroyed);
236  FVector GlobalPosition = LocalToGlobalLocation(Actor->GetActorLocation());
237  LM_LOG(Warning, "Actor Spawned at %s", *GlobalPosition.ToString());
238  }
239 
240 }
241 
243 {
244  if (ActorsToConsider.Num() > 0)
245  {
246  ACarlaWheeledVehicle* Hero = Cast<ACarlaWheeledVehicle>(ActorsToConsider[0]);
247  if (IsValid(Hero))
248  return Hero;
249  }
250  return nullptr;
251 }
252 
254 {
255  LM_LOG(Warning, "ALargeMapManager::OnActorDestroyed %s", *DestroyedActor->GetName());
256 
257  UWorld* World = GetWorld();
258  UCarlaEpisode* CarlaEpisode = UCarlaStatics::GetCurrentEpisode(World);
259  FCarlaActor* CarlaActor = CarlaEpisode->FindCarlaActor(DestroyedActor);
260  if (CarlaActor)
261  const FActorInfo* ActorInfo = CarlaActor->GetActorInfo();
262 
263  // Hero has been removed?
264  //
265 
266 }
267 
268 void ALargeMapManager::SetTile0Offset(const FVector& Offset)
269 {
270  Tile0Offset = Offset;
271 }
272 
274 {
275  TileSide = Size;
276 }
277 
279 {
280  return TileSide;
281 }
282 
284 {
285  return Tile0Offset;
286 }
287 
289 {
290  LayerStreamingDistance = Distance;
293 }
294 
296 {
297  ActorStreamingDistance = Distance;
300 }
301 
303 {
304  return LayerStreamingDistance;
305 }
306 
308 {
309  return ActorStreamingDistance;
310 }
311 
312 FTransform ALargeMapManager::GlobalToLocalTransform(const FTransform& InTransform) const
313 {
314  return FTransform(
315  InTransform.GetRotation(),
316  InTransform.GetLocation() - CurrentOriginD.ToFVector(),
317  InTransform.GetScale3D());
318 }
319 
320 FVector ALargeMapManager::GlobalToLocalLocation(const FVector& InLocation) const
321 {
322  return InLocation - CurrentOriginD.ToFVector();
323 }
324 
325 FTransform ALargeMapManager::LocalToGlobalTransform(const FTransform& InTransform) const
326 {
327  return FTransform(
328  InTransform.GetRotation(),
329  CurrentOriginD.ToFVector() + InTransform.GetLocation(),
330  InTransform.GetScale3D());
331 }
332 
333 FVector ALargeMapManager::LocalToGlobalLocation(const FVector& InLocation) const
334 {
335  return CurrentOriginD.ToFVector() + InLocation;
336 }
337 
339 uint64_t num_ticks = 0;
340 void ALargeMapManager::Tick(float DeltaTime)
341 {
342  Super::Tick(DeltaTime);
343 
344  // Update map tiles, load/unload based on actors to consider (heros) position
345  // Also, to avoid looping over the heros again, it checks if any actor to consider has been removed
347 
348  // Check if active actors are still in range and inside a loaded tile or have to be converted to dormant
350 
351  // Check if dormant actors have been moved to the load range
353 
354  // Remove the hero actors that doesn't exits any more from the ActorsToConsider vector
356 
358 
360 
362 
363 #if WITH_EDITOR
365 #endif // WITH_EDITOR
366 
367 }
368 
371 }
372 
373 void ALargeMapManager::GenerateMap(FString InAssetsPath)
374 {
375  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::GenerateMap);
376  LM_LOG(Warning, "Generating Map %s ...", *InAssetsPath);
378 
379  AssetsPath = InAssetsPath;
380 
381  /// Retrive all the assets in the path
382  TArray<FAssetData> AssetsData;
383  UObjectLibrary* ObjectLibrary = UObjectLibrary::CreateLibrary(UWorld::StaticClass(), true, true);
384  ObjectLibrary->LoadAssetDataFromPath(InAssetsPath);
385  ObjectLibrary->GetAssetDataList(AssetsData);
386 
387  /// Generate tiles based on mesh positions
388  UWorld* World = GetWorld();
389  MapTiles.Reset();
390  for (const FAssetData& AssetData : AssetsData)
391  {
392  #if WITH_EDITOR
393  // LM_LOG(Warning, "Loading asset name: %s", *(AssetData.AssetName.ToString()));
394  // LM_LOG(Warning, "Asset class: %s", *(AssetData.AssetClass.ToString()));
395  #endif
396  FString TileName = AssetData.AssetName.ToString();
397  if (!TileName.Contains("_Tile_"))
398  {
399  continue;
400  }
401  FString TileName_X = "";
402  FString TileName_Y = "";
403  size_t i = TileName.Len()-1;
404  for (; i > 0; i--) {
405  TCHAR character = TileName[i];
406  if (character == '_') {
407  break;
408  }
409  TileName_Y = FString::Chr(character) + TileName_Y;
410  }
411  i--;
412  for (; i > 0; i--) {
413  TCHAR character = TileName[i];
414  if (character == '_') {
415  break;
416  }
417  TileName_X = FString::Chr(character) + TileName_X;
418  }
419  FIntVector TileVectorID = FIntVector(FCString::Atoi(*TileName_X), FCString::Atoi(*TileName_Y), 0);
420  #if WITH_EDITOR
421  // LM_LOG(Warning, "Tile: %d, %d", TileVectorID.X, TileVectorID.Y);
422  #endif
423  TileID TileId = GetTileID(TileVectorID);
424  LoadCarlaMapTile(InAssetsPath + "/" + AssetData.AssetName.ToString(), TileId);
425  }
426  ObjectLibrary->ConditionalBeginDestroy();
427  GEngine->ForceGarbageCollection(true);
428 
429  ActorsToConsider.Reset();
430  if (Spectator)
431  {
433  }
434 
435 #if WITH_EDITOR
436  LM_LOG(Warning, "GenerateMap num Tiles generated %d", MapTiles.Num());
437  DumpTilesTable();
438 #endif // WITH_EDITOR
439 }
440 
441 void ALargeMapManager::GenerateMap(TArray<TPair<FString, FIntVector>> MapPathsIds)
442 {
443  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::GenerateMap);
444  LM_LOG(Warning, "Generating Large Map");
446 
447  for (TPair<FString, FIntVector>& PathId : MapPathsIds)
448  {
449  FIntVector& TileVectorID = PathId.Value;
450  FString& Path = PathId.Key;
451  TileID TileId = GetTileID(TileVectorID);
452  LoadCarlaMapTile(Path, TileId);
453  }
454 
455  #if WITH_EDITOR
456  LM_LOG(Warning, "GenerateMap num Tiles generated %d", MapTiles.Num());
457  #endif // WITH_EDITOR
458 }
459 
461 {
462  MapTiles.Empty();
463 }
464 
466 {
467  UWorld* World = GetWorld();
468  UWorldComposition* WorldComposition = World->WorldComposition;
469  World->ClearStreamingLevels();
470  WorldComposition->TilesStreaming.Empty();
471  WorldComposition->GetTilesList().Empty();
472 
473  for (auto& It : MapTiles)
474  {
475  ULevelStreamingDynamic* StreamingLevel = It.Value.StreamingLevel;
476  World->AddStreamingLevel(StreamingLevel);
477  WorldComposition->TilesStreaming.Add(StreamingLevel);
478  }
479 }
480 
481 // TODO: maybe remove this, I think I will not need it any more
482 void ALargeMapManager::AddActorToUnloadedList(const FCarlaActor& CarlaActor, const FTransform& Transform)
483 {
484  // ActiveActors.Add(CarlaActor.GetActorId(), {Transform, CarlaActor});
485 }
486 
488 {
489  int32 MinX = 0;
490  int32 MaxX = 0;
491  int32 MinY = 0;
492  int32 MaxY = 0;
493  for (auto& It : MapTiles)
494  {
495  FIntVector TileID = GetTileVectorID(It.Key);
496  MinX = (TileID.X < MinX) ? TileID.X : MinX;
497  MaxX = (TileID.X > MaxX) ? TileID.X : MaxX;
498 
499  MinY = (TileID.Y < MinY) ? TileID.Y : MinY;
500  MaxY = (TileID.Y > MaxY) ? TileID.Y : MaxY;
501  }
502  return { MaxX - MinX + 1, MaxY - MinY + 1, 0 };
503 }
504 
505 bool ALargeMapManager::IsLevelOfTileLoaded(FIntVector InTileID) const
506 {
507  TileID TileID = GetTileID(InTileID);
508 
509  const FCarlaMapTile* Tile = MapTiles.Find(TileID);
510  if (!Tile)
511  {
512  if (bPrintErrors)
513  {
514  LM_LOG(Warning, "IsLevelOfTileLoaded Tile %s does not exist", *InTileID.ToString());
515  }
516  return false;
517  }
518 
519  const ULevelStreamingDynamic* StreamingLevel = Tile->StreamingLevel;
520 
521  return (StreamingLevel && StreamingLevel->GetLoadedLevel());
522 }
523 
524 FIntVector ALargeMapManager::GetTileVectorID(FVector TileLocation) const
525 {
526  FIntVector VectorId = FIntVector(
527  (TileLocation -
528  (Tile0Offset - FVector(0.5f*TileSide,-0.5f*TileSide, 0) + LocalTileOffset))
529  / TileSide);
530  VectorId.Y *= -1;
531  return VectorId;
532 }
533 
534 FIntVector ALargeMapManager::GetTileVectorID(FDVector TileLocation) const
535 {
536  FIntVector VectorId = (
537  (TileLocation -
538  (Tile0Offset - FVector(0.5f*TileSide,-0.5f*TileSide, 0) + LocalTileOffset))
539  / TileSide).ToFIntVector();
540  VectorId.Y *= -1;
541  return VectorId;
542 }
543 
545 {
546  return FIntVector{
547  (int32)(TileID >> 32),
548  (int32)(TileID & (int32)(~0)),
549  0
550  };
551 }
552 
554 {
555  FIntVector VTileId = GetTileVectorID(TileID);
556  return GetTileLocation(VTileId);
557 }
558 
559 FVector ALargeMapManager::GetTileLocation(FIntVector TileVectorID) const
560 {
561  TileVectorID.Y *= -1;
562  return FVector(TileVectorID)* TileSide + Tile0Offset;
563 }
564 
566 {
567  FIntVector VTileId = GetTileVectorID(TileID);
568  return GetTileLocationD(VTileId);
569 }
570 
571 FDVector ALargeMapManager::GetTileLocationD(FIntVector TileVectorID) const
572 {
573  TileVectorID.Y *= -1;
574  return FDVector(TileVectorID) * TileSide + Tile0Offset;
575 }
576 
578 {
579  int64 X = ((int64)(TileVectorID.X) << 32);
580  int64 Y = (int64)(TileVectorID.Y) & 0x00000000FFFFFFFF;
581  return (X | Y);
582 }
583 
585 {
586  FIntVector TileID = GetTileVectorID(TileLocation);
587  return GetTileID(TileID);
588 }
589 
591 {
592  FIntVector TileID = GetTileVectorID(TileLocation);
593  return GetTileID(TileID);
594 }
595 
597 {
598  FCarlaMapTile* Tile = nullptr;
599  for (auto& It : MapTiles)
600  {
601  ULevelStreamingDynamic* StreamingLevel = It.Value.StreamingLevel;
602  ULevel* Level = StreamingLevel->GetLoadedLevel();
603  if (Level == InLevel)
604  {
605  Tile = &(It.Value);
606  break;
607  }
608  }
609  check(Tile);
610  return *Tile;
611 }
612 
614 {
615  TileID TileID = GetTileID(TileVectorID);
616  FCarlaMapTile* Tile = MapTiles.Find(TileID);
617  return Tile;
618 }
619 
621 {
622  FCarlaMapTile* Tile = MapTiles.Find(TileID);
623  return Tile;
624 }
625 
627  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::LoadCarlaMapTile);
628  // Need to generate a new Tile
629  FCarlaMapTile NewTile;
630  // 1 - Calculate the Tile position
631  FIntVector VTileID = GetTileVectorID(TileId);
632  NewTile.Location = GetTileLocation(TileId);
633  NewTile.Name = TileMapPath;
634 
635  // 3 - Generate the StreamLevel
636  FVector TileLocation = NewTile.Location;
637  FString TileName = NewTile.Name;
638  UWorld* World = GetWorld();
639  UWorldComposition* WorldComposition = World->WorldComposition;
640 
641  FString FullName = TileMapPath;
642  FString PackageFileName = FullName;
643  FString LongLevelPackageName = FPackageName::FilenameToLongPackageName(PackageFileName);
644  FString UniqueLevelPackageName = LongLevelPackageName;
645 
646  ULevelStreamingDynamic* StreamingLevel = NewObject<ULevelStreamingDynamic>(World, *TileName);
647  check(StreamingLevel);
648 
649  StreamingLevel->SetWorldAssetByPackageName(*UniqueLevelPackageName);
650 
651 #if WITH_EDITOR
652  if (World->IsPlayInEditor())
653  {
654  FWorldContext WorldContext = GEngine->GetWorldContextFromWorldChecked(World);
655  StreamingLevel->RenameForPIE(WorldContext.PIEInstance);
656  }
657  StreamingLevel->SetShouldBeVisibleInEditor(true);
658  StreamingLevel->LevelColor = FColor::MakeRandomColor();
659 #endif // WITH_EDITOR
660 
661  StreamingLevel->SetShouldBeLoaded(false);
662  StreamingLevel->SetShouldBeVisible(false);
663  StreamingLevel->bShouldBlockOnLoad = ShouldTilesBlockOnLoad;
664  StreamingLevel->bInitiallyLoaded = false;
665  StreamingLevel->bInitiallyVisible = false;
666  StreamingLevel->LevelTransform = FTransform(TileLocation);
667  StreamingLevel->PackageNameToLoad = *FullName;
668 
669  if (!FPackageName::DoesPackageExist(FullName, NULL, &PackageFileName))
670  {
671  LM_LOG(Error, "Level does not exist in package with FullName variable -> %s", *FullName);
672  }
673 
674  if (!FPackageName::DoesPackageExist(LongLevelPackageName, NULL, &PackageFileName))
675  {
676  LM_LOG(Error, "Level does not exist in package with LongLevelPackageName variable -> %s", *LongLevelPackageName);
677  }
678 
679  //Actual map package to load
680  StreamingLevel->PackageNameToLoad = *LongLevelPackageName;
681 
682  NewTile.StreamingLevel = StreamingLevel;
683 
684  // 4 - Add it to the map
685  return MapTiles.Add(TileId, NewTile);
686 }
687 
689 {
690  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::UpdateTilesState);
691  TSet<TileID> TilesToConsider;
692 
693  // Loop over ActorsToConsider to update the state of the map tiles
694  // if the actor is not valid will be removed
695  for (AActor* Actor : ActorsToConsider)
696  {
697  if (IsValid(Actor))
698  {
699  GetTilesToConsider(Actor, TilesToConsider);
700  }
701  else
702  {
703  ActorsToRemove.Add(Actor);
704  }
705  }
706 
707  TSet<TileID> TilesToBeVisible;
708  TSet<TileID> TilesToHidde;
709  GetTilesThatNeedToChangeState(TilesToConsider, TilesToBeVisible, TilesToHidde);
710 
711  UpdateTileState(TilesToBeVisible, true, true, true);
712 
713  UpdateTileState(TilesToHidde, false, false, false);
714 
715  UpdateCurrentTilesLoaded(TilesToBeVisible, TilesToHidde);
716 
717 }
718 
720 {
721  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::RemovePendingActorsToRemove);
722  if(ActorsToRemove.Num() > 0 || ActivesToRemove.Num() > 0)
723  {
724  LM_LOG(Log, "ActorsToRemove %d ActivesToRemove %d", ActorsToRemove.Num(), ActivesToRemove.Num());
725  }
726 
727  for (AActor* ActorToRemove : ActorsToRemove)
728  {
729  ActorsToConsider.Remove(ActorToRemove);
730  }
731  if(ActorsToConsider.Num() == 0 && Spectator)
732  {
734  }
735  ActorsToRemove.Reset();
736 
737 
738  for (FCarlaActor::IdType ActorToRemove : ActivesToRemove)
739  {
740  ActiveActors.Remove(ActorToRemove);
741  }
742  ActivesToRemove.Reset();
743 
745  {
746  DormantActors.Remove(Id);
747  }
748  DormantsToRemove.Reset();
749 }
750 
752 {
753  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::CheckActiveActors);
754  UWorld* World = GetWorld();
755  UCarlaEpisode* CarlaEpisode = UCarlaStatics::GetCurrentEpisode(World);
756  // Check if they have to be destroyed
758  {
759  FCarlaActor* View = CarlaEpisode->FindCarlaActor(Id);
760  if (View)
761  {
762  AActor * Actor = View->GetActor();
763  FVector RelativeLocation = Actor->GetActorLocation();
764  FDVector WorldLocation = CurrentOriginD + RelativeLocation;
765 
766  if(!IsTileLoaded(WorldLocation))
767  {
768  // Save to temporal container. Later will be converted to dormant
769  ActiveToDormantActors.Add(Id);
770  ActivesToRemove.Add(Id);
771  continue;
772  }
773 
774  for(AActor* HeroActor : ActorsToConsider)
775  {
776  FVector HeroLocation = HeroActor->GetActorLocation();
777 
778  float DistanceSquared = (RelativeLocation - HeroLocation).SizeSquared();
779 
780  if (DistanceSquared > ActorStreamingDistanceSquared)
781  {
782  // Save to temporal container. Later will be converted to dormant
783  ActiveToDormantActors.Add(Id);
784  ActivesToRemove.Add(Id);
785  }
786  }
787  }
788  else
789  {
790  LM_LOG(Warning, "CheckActiveActors Actor does not exist -> Remove actor");
791  ActivesToRemove.Add(Id);
792  }
793  }
794 }
795 
797 {
798  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::ConvertActiveToDormantActors);
799  UWorld* World = GetWorld();
800  UCarlaEpisode* CarlaEpisode = UCarlaStatics::GetCurrentEpisode(World);
801 
802  // These actors are on dormant state so remove them from active actors
803  // But save them on the dormant array first
805  {
806  // To dormant state
807  CarlaEpisode->PutActorToSleep(Id);
808 
809  LM_LOG(Warning, "Converting Active To Dormant... %d", Id);
810 
811  // Need the ID of the dormant actor and save it
812  DormantActors.Add(Id);
813  }
814 
815  ActiveToDormantActors.Reset();
816 }
817 
819 {
820  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::CheckDormantActors);
821  UWorld* World = GetWorld();
822  UCarlaEpisode* CarlaEpisode = UCarlaStatics::GetCurrentEpisode(World);
823 
824 
826  {
827  FCarlaActor* CarlaActor = CarlaEpisode->FindCarlaActor(Id);
828 
829  // If the Ids don't match, the actor has been removed
830  if(!CarlaActor)
831  {
832  LM_LOG(Log, "CheckDormantActors Carla Actor %d not found", Id);
833  DormantsToRemove.Add(Id);
834  continue;
835  }
836  if(CarlaActor->GetActorId() != Id)
837  {
838  LM_LOG(Warning, "CheckDormantActors IDs doesn't match!! Wanted = %d Received = %d", Id, CarlaActor->GetActorId());
839  DormantsToRemove.Add(Id);
840  continue;
841  }
842  if (!CarlaActor->IsDormant())
843  {
844  LM_LOG(Warning, "CheckDormantActors Carla Actor %d is not dormant", Id);
845  DormantsToRemove.Add(Id);
846  continue;
847  }
848 
849  const FActorData* ActorData = CarlaActor->GetActorData();
850 
852  {
853  FVector HeroLocation = Actor->GetActorLocation();
854 
855  FDVector WorldLocation = ActorData->Location;
856  FDVector RelativeLocation = WorldLocation - CurrentOriginD;
857 
858  float DistanceSquared = (RelativeLocation - HeroLocation).SizeSquared();
859 
860  if(DistanceSquared < ActorStreamingDistanceSquared && IsTileLoaded(WorldLocation))
861  {
862  DormantToActiveActors.Add(Id);
863  DormantsToRemove.Add(Id);
864  break;
865  }
866  }
867  }
868 }
869 
871 {
872  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::ConvertDormantToActiveActors);
873  UWorld* World = GetWorld();
874  UCarlaEpisode* CarlaEpisode = UCarlaStatics::GetCurrentEpisode(World);
875 
877  {
878  LM_LOG(Warning, "Converting %d Dormant To Active", Id);
879 
880  CarlaEpisode->WakeActorUp(Id);
881 
882  FCarlaActor* View = CarlaEpisode->FindCarlaActor(Id);
883 
884  if (View->IsActive()){
885  LM_LOG(Warning, "Spawning dormant at %s\n\tOrigin: %s\n\tRel. location: %s", \
886  *((CurrentOriginD + View->GetActor()->GetActorLocation()).ToString()), \
887  *(CurrentOriginD.ToString()), \
888  *((View->GetActor()->GetActorLocation()).ToString()) \
889  );
890  ActiveActors.Add(Id);
891  }
892  else
893  {
894  LM_LOG(Warning, "Actor %d could not be woken up, keeping sleep state", Id);
895  DormantActors.Add(Id);
896  }
897  }
898  DormantToActiveActors.Reset();
899 }
900 
902 {
903  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::CheckIfRebaseIsNeeded);
904  if(ActorsToConsider.Num() > 0)
905  {
906  UWorld* World = GetWorld();
907  UWorldComposition* WorldComposition = World->WorldComposition;
908  // TODO: consider multiple hero vehicles for rebasing
909  AActor* ActorToConsider = ActorsToConsider[0];
910  if( IsValid(ActorToConsider) )
911  {
912  FVector ActorLocation = ActorToConsider->GetActorLocation();
913  FIntVector ILocation = FIntVector(ActorLocation.X, ActorLocation.Y, ActorLocation.Z);
914  if (ActorLocation.SizeSquared() > FMath::Square(RebaseOriginDistance) )
915  {
916  TileID TileId = GetTileID(CurrentOriginD + ActorLocation);
917  FVector NewOrigin = GetTileLocation(TileId);
918  World->SetNewWorldOrigin(FIntVector(NewOrigin));
919  }
920  }
921  }
922 }
923 
924 void ALargeMapManager::GetTilesToConsider(const AActor* ActorToConsider,
925  TSet<TileID>& OutTilesToConsider)
926 {
927  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::GetTilesToConsider);
928  check(ActorToConsider);
929  // World location
930  FDVector ActorLocation = CurrentOriginD + ActorToConsider->GetActorLocation();
931 
932  // Calculate Current Tile
933  FIntVector CurrentTile = GetTileVectorID(ActorLocation);
934 
935  // Calculate tile bounds
936  FDVector UpperPos = ActorLocation + FDVector(LayerStreamingDistance,LayerStreamingDistance,0);
937  FDVector LowerPos = ActorLocation + FDVector(-LayerStreamingDistance,-LayerStreamingDistance,0);
938  FIntVector UpperTileId = GetTileVectorID(UpperPos);
939  FIntVector LowerTileId = GetTileVectorID(LowerPos);
940  for (int Y = UpperTileId.Y; Y <= LowerTileId.Y; Y++)
941  {
942  for (int X = LowerTileId.X; X <= UpperTileId.X; X++)
943  {
944  // I don't check the bounds of the Tile map, if the Tile does not exist
945  // I just simply discard it
946  FIntVector TileToCheck = FIntVector(X, Y, 0);
947 
948  TileID TileID = GetTileID(TileToCheck);
949  FCarlaMapTile* Tile = MapTiles.Find(TileID);
950  if (!Tile)
951  {
952  // LM_LOG(Warning, "Requested tile %d, %d but tile was not found", TileToCheck.X, TileToCheck.Y);
953  continue; // Tile does not exist, discard
954  }
955 
956  OutTilesToConsider.Add(TileID);
957  }
958  }
959 }
960 
962  const TSet<TileID>& InTilesToConsider,
963  TSet<TileID>& OutTilesToBeVisible,
964  TSet<TileID>& OutTilesToHidde)
965 {
966  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::GetTilesThatNeedToChangeState);
967  OutTilesToBeVisible = InTilesToConsider.Difference(CurrentTilesLoaded);
968  OutTilesToHidde = CurrentTilesLoaded.Difference(InTilesToConsider);
969 }
970 
972  const TSet<TileID>& InTilesToUpdate,
973  bool InShouldBlockOnLoad,
974  bool InShouldBeLoaded,
975  bool InShouldBeVisible)
976 {
977  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::UpdateTileState);
978  UWorld* World = GetWorld();
979  UWorldComposition* WorldComposition = World->WorldComposition;
980 
981  // Gather all the locations of the levels to load
982  for (const TileID TileID : InTilesToUpdate)
983  {
984  FCarlaMapTile* CarlaTile = MapTiles.Find(TileID);
985  check(CarlaTile); // If an invalid ID reach here, we did something very wrong
986  ULevelStreamingDynamic* StreamingLevel = CarlaTile->StreamingLevel;
987  StreamingLevel->bShouldBlockOnLoad = InShouldBlockOnLoad;
988  StreamingLevel->SetShouldBeLoaded(InShouldBeLoaded);
989  StreamingLevel->SetShouldBeVisible(InShouldBeVisible);
990  }
991 }
992 
994  const TSet<TileID>& InTilesToBeVisible,
995  const TSet<TileID>& InTilesToHidde)
996 {
997  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::UpdateCurrentTilesLoaded);
998  for (const TileID TileID : InTilesToHidde)
999  {
1000  CurrentTilesLoaded.Remove(TileID);
1001  }
1002 
1003  for (const TileID TileID : InTilesToBeVisible)
1004  {
1006  }
1007 }
1008 
1010 {
1011  int32 X = (int32)(TileID >> 32);
1012  int32 Y = (int32)(TileID);
1013  return FString::Printf(TEXT("Tile_%d_%d"), X, Y);
1014 }
1015 
1017 {
1018  int32 X = (int32)(TileID >> 32);
1019  int32 Y = (int32)(TileID);
1020  return FString::Printf(TEXT("%d_%d"), X, Y);
1021 }
1022 
1024 {
1025  FString FileContent = "";
1026  FileContent += FString::Printf(TEXT("LargeMapManager state\n"));
1027 
1028  FileContent += FString::Printf(TEXT("Tile:\n"));
1029  FileContent += FString::Printf(TEXT("ID\tName\tLocation\n"));
1030  for (auto& It : MapTiles)
1031  {
1032  const FCarlaMapTile& Tile = It.Value;
1033  FileContent += FString::Printf(TEXT(" %ld\t%s\t%s\n"), It.Key, *Tile.Name, *Tile.Location.ToString());
1034  }
1035  FileContent += FString::Printf(TEXT("\nNum generated tiles: %d\n"), MapTiles.Num());
1036 
1037  // Generate the map name with the assets folder name
1038  TArray<FString> StringArray;
1039  AssetsPath.ParseIntoArray(StringArray, TEXT("/"), false);
1040 
1041  FString FilePath = FPaths::ProjectSavedDir() + StringArray[StringArray.Num() - 1] + ".txt";
1042  FFileHelper::SaveStringToFile(
1043  FileContent,
1044  *FilePath,
1045  FFileHelper::EEncodingOptions::AutoDetect,
1046  &IFileManager::Get(),
1047  EFileWrite::FILEWRITE_Silent);
1048 }
1049 
1051 {
1052  UWorld* World = GetWorld();
1053 
1054  FDVector CurrentActorPosition;
1055  if (ActorsToConsider.Num() > 0)
1056  CurrentActorPosition = CurrentOriginD + ActorsToConsider[0]->GetActorLocation();
1057 
1058  const TArray<FLevelCollection>& WorldLevelCollections = World->GetLevelCollections();
1059  const FLevelCollection* LevelCollection = World->GetActiveLevelCollection();
1060  const TArray<ULevel*>& Levels = World->GetLevels();
1061  const TArray<ULevelStreaming*>& StreamingLevels = World->GetStreamingLevels();
1062  ULevel* CurrentLevel = World->GetCurrentLevel();
1063 
1064  FString Output = "";
1065  Output += FString::Printf(TEXT("Num levels in world composition: %d\n"), World->WorldComposition->TilesStreaming.Num());
1066  Output += FString::Printf(TEXT("Num levels loaded: %d\n"), Levels.Num() );
1067  Output += FString::Printf(TEXT("Num tiles loaded: %d\n"), CurrentTilesLoaded.Num() );
1068  Output += FString::Printf(TEXT("Tiles loaded: [ "));
1069  for(TileID& TileId : CurrentTilesLoaded)
1070  {
1071  Output += FString::Printf(TEXT("%s, "), *TileIDToString(TileId));
1072  }
1073  Output += FString::Printf(TEXT("]\n"));
1074  GEngine->AddOnScreenDebugMessage(0, MsgTime, FColor::Cyan, Output);
1075 
1076  int LastMsgIndex = TilesDistMsgIndex;
1077  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White,
1078  FString::Printf(TEXT("\nActor Global Position: %s km"), *(FDVector(CurrentActorPosition) / (1000.0 * 100.0)).ToString()) );
1079 
1080  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White, TEXT("Closest tiles - Distance:"));
1081 
1082  for (const auto& TilePair : MapTiles)
1083  {
1084  const FCarlaMapTile& Tile = TilePair.Value;
1085  const ULevelStreaming* Level = Tile.StreamingLevel;
1086  FVector LevelLocation = Tile.Location;
1087  float Distance = FDVector::Dist(LevelLocation, CurrentActorPosition);
1088 
1089  FColor MsgColor = FColor::Green;
1090  if (Distance < (TileSide * 2.0f))
1091  {
1092  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, MsgColor,
1093  FString::Printf(TEXT("%s %.2f"), *Level->GetName(), Distance / (1000.0f * 100.0f)));
1094  }
1095  if (LastMsgIndex < MaxTilesDistMsgIndex) break;
1096  }
1097  LastMsgIndex = ClientLocMsgIndex;
1098  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White,
1099  FString::Printf(TEXT("\nOrigin: %s km"), *(FDVector(CurrentOriginInt) / (1000.0 * 100.0)).ToString()) );
1100  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White,
1101  FString::Printf(TEXT("Num active actors (%d)"), ActiveActors.Num()) );
1102  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White,
1103  FString::Printf(TEXT("Num dormant actors (%d)"), DormantActors.Num()));
1104  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White,
1105  FString::Printf(TEXT("Actors To Consider (%d)"), ActorsToConsider.Num()));
1106  for (const AActor* Actor : ActorsToConsider)
1107  {
1108  if (IsValid(Actor))
1109  {
1110  Output = "";
1111  float ToKm = 1000.0f * 100.0f;
1112  FVector TileActorLocation = Actor->GetActorLocation();
1113  FDVector ClientActorLocation = CurrentOriginD + FDVector(TileActorLocation);
1114 
1115  Output += FString::Printf(TEXT("Local Loc: %s meters\n"), *(TileActorLocation / ToKm).ToString());
1116  Output += FString::Printf(TEXT("Client Loc: %s km\n"), *(ClientActorLocation / ToKm).ToString());
1117  Output += "---------------";
1118  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, PositonMsgColor, Output);
1119 
1120  if (LastMsgIndex > MaxClientLocMsgIndex) break;
1121  }
1122  }
1123 }
FCarlaActor * FindCarlaActor(FCarlaActor::IdType ActorId)
Find a Carla actor by id.
Definition: CarlaEpisode.h:172
TileID GetTileID(FVector TileLocation) const
From a given location it retrieves the TileID that covers that area.
double Y
Definition: DVector.h:14
TSet< uint64 > CurrentTilesLoaded
FString GenerateTileName(TileID TileID)
void UpdateCurrentTilesLoaded(const TSet< TileID > &InTilesToBeVisible, const TSet< TileID > &InTilesToHidde)
A registry of all the Carla actors.
Definition: ActorRegistry.h:20
const int32 MaxTilesDistMsgIndex
void GenerateMap(FString InAssetsPath)
TSet< FCarlaActor::IdType > DormantsToRemove
TSet< FCarlaActor::IdType > DormantToActiveActors
void ConvertActiveToDormantActors()
static double Dist(const FDVector &V1, const FDVector &V2)
Definition: DVector.h:37
FIntVector GetNumTilesInXY() const
FString ToString() const
Definition: DVector.h:52
AActor * GetActor()
Definition: CarlaActor.h:90
FVector GlobalToLocalLocation(const FVector &InLocation) const
void ConvertDormantToActiveActors()
float LayerStreamingDistanceSquared
ACarlaWheeledVehicle * GetHeroVehicle()
FCarlaMapTile & GetCarlaMapTile(FVector Location)
void WakeActorUp(carla::rpc::ActorId ActorId)
Definition: CarlaEpisode.h:284
std::vector< cg::Location > Path
void RegisterTilesInWorldComposition()
TSet< FCarlaActor::IdType > ActiveToDormantActors
const int32 TilesDistMsgIndex
FCarlaMapTile & LoadCarlaMapTile(FString TileMapPath, TileID TileId)
FString TileIDToString(TileID TileID)
uint32 IdType
Definition: CarlaActor.h:27
bool IsLevelOfTileLoaded(FIntVector InTileID) const
static bool IsValid(const ACarlaWheeledVehicle *Vehicle)
void PutActorToSleep(carla::rpc::ActorId ActorId)
Definition: CarlaEpisode.h:279
void OnActorSpawned(const FCarlaActor &CarlaActor)
FIntVector CurrentOriginInt
static T Get(carla::rpc::Response< T > &response)
void UpdateTileState(const TSet< TileID > &InTilesToUpdate, bool InShouldBlockOnLoad, bool InShouldBeLoaded, bool InShouldBeVisible)
virtual void BeginPlay() override
uint64_t num_ticks
float ActorStreamingDistanceSquared
FVector ToFVector() const
Definition: DVector.h:47
static void TagActorsInLevel(UWorld &World, bool bTagForSemanticSegmentation)
Set the tag of every actor in level.
Definition: Tagger.cpp:220
const FActorRegistry & GetActorRegistry() const
Definition: CarlaEpisode.h:154
TMap< FString, FActorAttribute > Variations
User selected variations of the actor.
FActorData * GetActorData()
Definition: CarlaActor.h:156
carla::SharedPtr< cc::Actor > Actor
A simulation episode.
Definition: CarlaEpisode.h:37
void GetTilesThatNeedToChangeState(const TSet< TileID > &InTilesToConsider, TSet< TileID > &OutTilesToBeVisible, TSet< TileID > &OutTilesToHidde)
float tick_execution_time
const int32 ClientLocMsgIndex
void SetActorStreamingDistance(float Distance)
void PreWorldOriginOffset(UWorld *InWorld, FIntVector InSrcOrigin, FIntVector InDstOrigin)
bool IsActive() const
Definition: CarlaActor.h:65
void SetTileSize(float Size)
void DumpTilesTable() const
A description of a Carla Actor with all its variation.
FActorDescription Description
Definition: ActorInfo.h:26
const FActorInfo * GetActorInfo() const
Definition: CarlaActor.h:100
TMap< uint64, FCarlaMapTile > MapTiles
void SetLayerStreamingDistance(float Distance)
void SetCurrentMapOrigin(const FIntVector &NewOrigin)
Definition: CarlaEpisode.h:323
void GetTilesToConsider(const AActor *ActorToConsider, TSet< TileID > &OutTilesToConsider)
FDVector GetTileLocationD(TileID TileID) const
FTransform LocalToGlobalTransform(const FTransform &InTransform) const
TArray< AActor * > ActorsToConsider
void AddActorToUnloadedList(const FCarlaActor &CarlaActor, const FTransform &Transform)
#define LM_LOG(Level, Msg,...)
float GetActorStreamingDistance() const
float GetLayerStreamingDistance() const
FTransform GlobalToLocalTransform(const FTransform &InTransform) const
void PostWorldOriginOffset(UWorld *InWorld, FIntVector InSrcOrigin, FIntVector InDstOrigin)
static UCarlaEpisode * GetCurrentEpisode(const UObject *WorldContextObject)
Definition: CarlaStatics.h:68
void OnLevelRemovedFromWorld(ULevel *InLevel, UWorld *InWorld)
A view over an actor and its properties.
Definition: ActorInfo.h:22
bool IsDormant() const
Definition: CarlaActor.h:70
TSet< FCarlaActor::IdType > ActivesToRemove
TSet< AActor * > ActorsToRemove
const int32 MaxClientLocMsgIndex
IdType GetActorId() const
Definition: CarlaActor.h:80
ULevelStreamingDynamic * StreamingLevel
void RemovePendingActorsToRemove()
void SetTile0Offset(const FVector &Offset)
TArray< FCarlaActor::IdType > ActiveActors
Base class for CARLA wheeled vehicles.
void OnActorDestroyed(AActor *DestroyedActor)
An actor attribute, may be an intrinsic (non-modifiable) attribute of the actor or an user-defined ac...
void Tick(float DeltaTime) override
float RebaseOriginDistanceSquared
FVector GetTileLocation(TileID TileID) const
geom::Transform Transform
Definition: rpc/Transform.h:16
FVector LocalToGlobalLocation(const FVector &InLocation) const
bool IsTileLoaded(TileID TileId) const
ActorType GetActorType() const
Definition: CarlaActor.h:85
FIntVector GetTileVectorID(FVector TileLocation) const
A view over an actor and its properties.
Definition: CarlaActor.h:23
TArray< FCarlaActor::IdType > DormantActors
void OnLevelAddedToWorld(ULevel *InLevel, UWorld *InWorld)
FDVector Location
Definition: ActorData.h:31