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