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  LayerStreamingDistance = Distance;
242 }
243 
245 {
246  ActorStreamingDistance = Distance;
249 }
250 
252 {
253  return LayerStreamingDistance;
254 }
255 
257 {
258  return ActorStreamingDistance;
259 }
260 
261 FTransform ALargeMapManager::GlobalToLocalTransform(const FTransform& InTransform) const
262 {
263  return FTransform(
264  InTransform.GetRotation(),
265  InTransform.GetLocation() - CurrentOriginD.ToFVector(),
266  InTransform.GetScale3D());
267 }
268 
269 FVector ALargeMapManager::GlobalToLocalLocation(const FVector& InLocation) const
270 {
271  return InLocation - CurrentOriginD.ToFVector();
272 }
273 
274 FTransform ALargeMapManager::LocalToGlobalTransform(const FTransform& InTransform) const
275 {
276  return FTransform(
277  InTransform.GetRotation(),
278  CurrentOriginD.ToFVector() + InTransform.GetLocation(),
279  InTransform.GetScale3D());
280 }
281 
282 FVector ALargeMapManager::LocalToGlobalLocation(const FVector& InLocation) const
283 {
284  return CurrentOriginD.ToFVector() + InLocation;
285 }
286 
288 uint64_t num_ticks = 0;
289 void ALargeMapManager::Tick(float DeltaTime)
290 {
291  Super::Tick(DeltaTime);
292 
293  // Update map tiles, load/unload based on actors to consider (heros) position
294  // Also, to avoid looping over the heros again, it checks if any actor to consider has been removed
296 
297  // Check if active actors are still in range and inside a loaded tile or have to be converted to dormant
299 
300  // Check if dormant actors have been moved to the load range
302 
303  // Remove the hero actors that doesn't exits any more from the ActorsToConsider vector
305 
307 
309 
311 
312 #if WITH_EDITOR
314 #endif // WITH_EDITOR
315 
316 }
317 
320 }
321 
322 void ALargeMapManager::GenerateMap(FString InAssetsPath)
323 {
324  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::GenerateMap);
325  LM_LOG(Warning, "Generating Map %s ...", *InAssetsPath);
327 
328  AssetsPath = InAssetsPath;
329 
330  /// Retrive all the assets in the path
331  TArray<FAssetData> AssetsData;
332  UObjectLibrary* ObjectLibrary = UObjectLibrary::CreateLibrary(UWorld::StaticClass(), true, true);
333  ObjectLibrary->LoadAssetDataFromPath(InAssetsPath);
334  ObjectLibrary->GetAssetDataList(AssetsData);
335 
336  /// Generate tiles based on mesh positions
337  UWorld* World = GetWorld();
338  MapTiles.Reset();
339  for (const FAssetData& AssetData : AssetsData)
340  {
341  #if WITH_EDITOR
342  // LM_LOG(Warning, "Loading asset name: %s", *(AssetData.AssetName.ToString()));
343  // LM_LOG(Warning, "Asset class: %s", *(AssetData.AssetClass.ToString()));
344  #endif
345  FString TileName = AssetData.AssetName.ToString();
346  if (!TileName.Contains("_Tile_"))
347  {
348  continue;
349  }
350  FString TileName_X = "";
351  FString TileName_Y = "";
352  size_t i = TileName.Len()-1;
353  for (; i > 0; i--) {
354  TCHAR character = TileName[i];
355  if (character == '_') {
356  break;
357  }
358  TileName_Y = FString::Chr(character) + TileName_Y;
359  }
360  i--;
361  for (; i > 0; i--) {
362  TCHAR character = TileName[i];
363  if (character == '_') {
364  break;
365  }
366  TileName_X = FString::Chr(character) + TileName_X;
367  }
368  FIntVector TileVectorID = FIntVector(FCString::Atoi(*TileName_X), FCString::Atoi(*TileName_Y), 0);
369  #if WITH_EDITOR
370  // LM_LOG(Warning, "Tile: %d, %d", TileVectorID.X, TileVectorID.Y);
371  #endif
372  TileID TileId = GetTileID(TileVectorID);
373  LoadCarlaMapTile(InAssetsPath + "/" + AssetData.AssetName.ToString(), TileId);
374  }
375  ObjectLibrary->ConditionalBeginDestroy();
376  GEngine->ForceGarbageCollection(true);
377 
378  ActorsToConsider.Reset();
379 
380 #if WITH_EDITOR
381  LM_LOG(Warning, "GenerateMap num Tiles generated %d", MapTiles.Num());
382  DumpTilesTable();
383 #endif // WITH_EDITOR
384 }
385 
386 void ALargeMapManager::GenerateMap(TArray<TPair<FString, FIntVector>> MapPathsIds)
387 {
388  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::GenerateMap);
389  LM_LOG(Warning, "Generating Large Map");
391 
392  for (TPair<FString, FIntVector>& PathId : MapPathsIds)
393  {
394  FIntVector& TileVectorID = PathId.Value;
395  FString& Path = PathId.Key;
396  TileID TileId = GetTileID(TileVectorID);
397  LoadCarlaMapTile(Path, TileId);
398  }
399 
400  #if WITH_EDITOR
401  LM_LOG(Warning, "GenerateMap num Tiles generated %d", MapTiles.Num());
402  #endif // WITH_EDITOR
403 }
404 
406 {
407  MapTiles.Empty();
408 }
409 
411 {
412  UWorld* World = GetWorld();
413  UWorldComposition* WorldComposition = World->WorldComposition;
414  World->ClearStreamingLevels();
415  WorldComposition->TilesStreaming.Empty();
416  WorldComposition->GetTilesList().Empty();
417 
418  for (auto& It : MapTiles)
419  {
420  ULevelStreamingDynamic* StreamingLevel = It.Value.StreamingLevel;
421  World->AddStreamingLevel(StreamingLevel);
422  WorldComposition->TilesStreaming.Add(StreamingLevel);
423  }
424 }
425 
426 // TODO: maybe remove this, I think I will not need it any more
427 void ALargeMapManager::AddActorToUnloadedList(const FCarlaActor& CarlaActor, const FTransform& Transform)
428 {
429  // ActiveActors.Add(CarlaActor.GetActorId(), {Transform, CarlaActor});
430 }
431 
433 {
434  int32 MinX = 0;
435  int32 MaxX = 0;
436  int32 MinY = 0;
437  int32 MaxY = 0;
438  for (auto& It : MapTiles)
439  {
440  FIntVector TileID = GetTileVectorID(It.Key);
441  MinX = (TileID.X < MinX) ? TileID.X : MinX;
442  MaxX = (TileID.X > MaxX) ? TileID.X : MaxX;
443 
444  MinY = (TileID.Y < MinY) ? TileID.Y : MinY;
445  MaxY = (TileID.Y > MaxY) ? TileID.Y : MaxY;
446  }
447  return { MaxX - MinX + 1, MaxY - MinY + 1, 0 };
448 }
449 
450 bool ALargeMapManager::IsLevelOfTileLoaded(FIntVector InTileID) const
451 {
452  TileID TileID = GetTileID(InTileID);
453 
454  const FCarlaMapTile* Tile = MapTiles.Find(TileID);
455  if (!Tile)
456  {
457  if (bPrintErrors)
458  {
459  LM_LOG(Warning, "IsLevelOfTileLoaded Tile %s does not exist", *InTileID.ToString());
460  }
461  return false;
462  }
463 
464  const ULevelStreamingDynamic* StreamingLevel = Tile->StreamingLevel;
465 
466  return (StreamingLevel && StreamingLevel->GetLoadedLevel());
467 }
468 
469 FIntVector ALargeMapManager::GetTileVectorID(FVector TileLocation) const
470 {
471  FIntVector VectorId = FIntVector(
472  (TileLocation -
473  (Tile0Offset - FVector(0.5f*TileSide,-0.5f*TileSide, 0)))
474  / TileSide);
475  VectorId.Y *= -1;
476  return VectorId;
477 }
478 
479 FIntVector ALargeMapManager::GetTileVectorID(FDVector TileLocation) const
480 {
481  FIntVector VectorId = (
482  (TileLocation -
483  (Tile0Offset - FVector(0.5f*TileSide,-0.5f*TileSide, 0)))
484  / TileSide).ToFIntVector();
485  VectorId.Y *= -1;
486  return VectorId;
487 }
488 
490 {
491  return FIntVector{
492  (int32)(TileID >> 32),
493  (int32)(TileID & (int32)(~0)),
494  0
495  };
496 }
497 
499 {
500  FIntVector VTileId = GetTileVectorID(TileID);
501  return GetTileLocation(VTileId);
502 }
503 
504 FVector ALargeMapManager::GetTileLocation(FIntVector TileVectorID) const
505 {
506  TileVectorID.Y *= -1;
507  return FVector(TileVectorID)* TileSide + Tile0Offset;
508 }
509 
511 {
512  FIntVector VTileId = GetTileVectorID(TileID);
513  return GetTileLocationD(VTileId);
514 }
515 
516 FDVector ALargeMapManager::GetTileLocationD(FIntVector TileVectorID) const
517 {
518  TileVectorID.Y *= -1;
519  return FDVector(TileVectorID) * TileSide + Tile0Offset;
520 }
521 
523 {
524  int64 X = ((int64)(TileVectorID.X) << 32);
525  int64 Y = (int64)(TileVectorID.Y) & 0x00000000FFFFFFFF;
526  return (X | Y);
527 }
528 
530 {
531  FIntVector TileID = GetTileVectorID(TileLocation);
532  return GetTileID(TileID);
533 }
534 
536 {
537  FIntVector TileID = GetTileVectorID(TileLocation);
538  return GetTileID(TileID);
539 }
540 
542 {
543  FCarlaMapTile* Tile = nullptr;
544  for (auto& It : MapTiles)
545  {
546  ULevelStreamingDynamic* StreamingLevel = It.Value.StreamingLevel;
547  ULevel* Level = StreamingLevel->GetLoadedLevel();
548  if (Level == InLevel)
549  {
550  Tile = &(It.Value);
551  break;
552  }
553  }
554  check(Tile);
555  return *Tile;
556 }
557 
559 {
560  TileID TileID = GetTileID(TileVectorID);
561  FCarlaMapTile* Tile = MapTiles.Find(TileID);
562  return Tile;
563 }
564 
566  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::LoadCarlaMapTile);
567  // Need to generate a new Tile
568  FCarlaMapTile NewTile;
569  // 1 - Calculate the Tile position
570  FIntVector VTileID = GetTileVectorID(TileId);
571  NewTile.Location = GetTileLocation(TileId);
572  NewTile.Name = TileMapPath;
573 
574  // 3 - Generate the StreamLevel
575  FVector TileLocation = NewTile.Location;
576  FString TileName = NewTile.Name;
577  UWorld* World = GetWorld();
578  UWorldComposition* WorldComposition = World->WorldComposition;
579 
580  FString FullName = TileMapPath;
581  FString PackageFileName = FullName;
582  FString LongLevelPackageName = FPackageName::FilenameToLongPackageName(PackageFileName);
583  FString UniqueLevelPackageName = LongLevelPackageName;
584 
585  ULevelStreamingDynamic* StreamingLevel = NewObject<ULevelStreamingDynamic>(World, *TileName);
586  check(StreamingLevel);
587 
588  StreamingLevel->SetWorldAssetByPackageName(*UniqueLevelPackageName);
589 
590 #if WITH_EDITOR
591  if (World->IsPlayInEditor())
592  {
593  FWorldContext WorldContext = GEngine->GetWorldContextFromWorldChecked(World);
594  StreamingLevel->RenameForPIE(WorldContext.PIEInstance);
595  }
596  StreamingLevel->SetShouldBeVisibleInEditor(true);
597  StreamingLevel->LevelColor = FColor::MakeRandomColor();
598 #endif // WITH_EDITOR
599 
600  StreamingLevel->SetShouldBeLoaded(false);
601  StreamingLevel->SetShouldBeVisible(false);
602  StreamingLevel->bShouldBlockOnLoad = ShouldTilesBlockOnLoad;
603  StreamingLevel->bInitiallyLoaded = false;
604  StreamingLevel->bInitiallyVisible = false;
605  StreamingLevel->LevelTransform = FTransform(TileLocation);
606  StreamingLevel->PackageNameToLoad = *FullName;
607 
608  if (!FPackageName::DoesPackageExist(FullName, NULL, &PackageFileName))
609  {
610  LM_LOG(Error, "Level does not exist in package with FullName variable -> %s", *FullName);
611  }
612 
613  if (!FPackageName::DoesPackageExist(LongLevelPackageName, NULL, &PackageFileName))
614  {
615  LM_LOG(Error, "Level does not exist in package with LongLevelPackageName variable -> %s", *LongLevelPackageName);
616  }
617 
618  //Actual map package to load
619  StreamingLevel->PackageNameToLoad = *LongLevelPackageName;
620 
621  NewTile.StreamingLevel = StreamingLevel;
622 
623  // 4 - Add it to the map
624  return MapTiles.Add(TileId, NewTile);
625 }
626 
628 {
629  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::UpdateTilesState);
630  TSet<TileID> TilesToConsider;
631 
632  // Loop over ActorsToConsider to update the state of the map tiles
633  // if the actor is not valid will be removed
634  for (AActor* Actor : ActorsToConsider)
635  {
636  if (IsValid(Actor))
637  {
638  GetTilesToConsider(Actor, TilesToConsider);
639  }
640  else
641  {
642  ActorsToRemove.Add(Actor);
643  }
644  }
645 
646  TSet<TileID> TilesToBeVisible;
647  TSet<TileID> TilesToHidde;
648  GetTilesThatNeedToChangeState(TilesToConsider, TilesToBeVisible, TilesToHidde);
649 
650  UpdateTileState(TilesToBeVisible, true, true, true);
651 
652  UpdateTileState(TilesToHidde, false, false, false);
653 
654  UpdateCurrentTilesLoaded(TilesToBeVisible, TilesToHidde);
655 
656 }
657 
659 {
660  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::RemovePendingActorsToRemove);
661  if(ActorsToRemove.Num() > 0 || ActivesToRemove.Num() > 0)
662  {
663  LM_LOG(Error, "ActorsToRemove %d ActivesToRemove %d", ActorsToRemove.Num(), ActivesToRemove.Num());
664  }
665 
666  for (AActor* ActorToRemove : ActorsToRemove)
667  {
668  ActorsToConsider.Remove(ActorToRemove);
669  }
670  ActorsToRemove.Reset();
671 
672 
673  for (FCarlaActor::IdType ActorToRemove : ActivesToRemove)
674  {
675  ActiveActors.Remove(ActorToRemove);
676  }
677  ActivesToRemove.Reset();
678 
680  {
681  DormantActors.Remove(Id);
682  }
683  DormantsToRemove.Reset();
684 }
685 
687 {
688  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::CheckActiveActors);
689  UWorld* World = GetWorld();
690  UCarlaEpisode* CarlaEpisode = UCarlaStatics::GetCurrentEpisode(World);
691  // Check if they have to be destroyed
693  {
694  FCarlaActor* View = CarlaEpisode->FindCarlaActor(Id);
695  if (View)
696  {
697  AActor * Actor = View->GetActor();
698  FVector RelativeLocation = Actor->GetActorLocation();
699  FDVector WorldLocation = CurrentOriginD + RelativeLocation;
700 
701  if(!IsTileLoaded(WorldLocation))
702  {
703  // Save to temporal container. Later will be converted to dormant
704  ActiveToDormantActors.Add(Id);
705  ActivesToRemove.Add(Id);
706  continue;
707  }
708 
709  for(AActor* HeroActor : ActorsToConsider)
710  {
711  FVector HeroLocation = HeroActor->GetActorLocation();
712 
713  float DistanceSquared = (RelativeLocation - HeroLocation).SizeSquared();
714 
715  if (DistanceSquared > ActorStreamingDistanceSquared)
716  {
717  // Save to temporal container. Later will be converted to dormant
718  ActiveToDormantActors.Add(Id);
719  ActivesToRemove.Add(Id);
720  }
721  }
722  }
723  else
724  {
725  LM_LOG(Warning, "CheckActiveActors Actor does not exist -> Remove actor");
726  ActivesToRemove.Add(Id);
727  }
728  }
729 }
730 
732 {
733  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::ConvertActiveToDormantActors);
734  UWorld* World = GetWorld();
735  UCarlaEpisode* CarlaEpisode = UCarlaStatics::GetCurrentEpisode(World);
736 
737  // These actors are on dormant state so remove them from active actors
738  // But save them on the dormant array first
740  {
741  // To dormant state
742  CarlaEpisode->PutActorToSleep(Id);
743 
744  LM_LOG(Warning, "Converting Active To Dormant... %d", Id);
745 
746  // Need the ID of the dormant actor and save it
747  DormantActors.Add(Id);
748  }
749 
750  ActiveToDormantActors.Reset();
751 }
752 
754 {
755  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::CheckDormantActors);
756  UWorld* World = GetWorld();
757  UCarlaEpisode* CarlaEpisode = UCarlaStatics::GetCurrentEpisode(World);
758 
759 
761  {
762  FCarlaActor* CarlaActor = CarlaEpisode->FindCarlaActor(Id);
763 
764  // If the Ids don't match, the actor has been removed
765  if(!CarlaActor)
766  {
767  LM_LOG(Error, "CheckDormantActors Carla Actor %d not found", Id);
768  DormantsToRemove.Add(Id);
769  continue;
770  }
771  if(CarlaActor->GetActorId() != Id)
772  {
773  LM_LOG(Error, "CheckDormantActors IDs doesn't match!! Wanted = %d Received = %d", Id, CarlaActor->GetActorId());
774  DormantsToRemove.Add(Id);
775  continue;
776  }
777  if (!CarlaActor->IsDormant())
778  {
779  LM_LOG(Error, "CheckDormantActors Carla Actor %d is not dormant", Id);
780  DormantsToRemove.Add(Id);
781  continue;
782  }
783 
784  const FActorData* ActorData = CarlaActor->GetActorData();
785 
787  {
788  FVector HeroLocation = Actor->GetActorLocation();
789 
790  FDVector WorldLocation = ActorData->Location;
791  FDVector RelativeLocation = WorldLocation - CurrentOriginD;
792 
793  float DistanceSquared = (RelativeLocation - HeroLocation).SizeSquared();
794 
795  if(DistanceSquared < ActorStreamingDistanceSquared && IsTileLoaded(WorldLocation))
796  {
797  DormantToActiveActors.Add(Id);
798  DormantsToRemove.Add(Id);
799  break;
800  }
801  }
802  }
803 }
804 
806 {
807  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::ConvertDormantToActiveActors);
808  UWorld* World = GetWorld();
809  UCarlaEpisode* CarlaEpisode = UCarlaStatics::GetCurrentEpisode(World);
810 
812  {
813  LM_LOG(Warning, "Converting %d Dormant To Active", Id);
814 
815  CarlaEpisode->WakeActorUp(Id);
816 
817  FCarlaActor* View = CarlaEpisode->FindCarlaActor(Id);
818 
819  if (View->IsActive()){
820  LM_LOG(Warning, "Spawning dormant at %s\n\tOrigin: %s\n\tRel. location: %s", \
821  *((CurrentOriginD + View->GetActor()->GetActorLocation()).ToString()), \
822  *(CurrentOriginD.ToString()), \
823  *((View->GetActor()->GetActorLocation()).ToString()) \
824  );
825  ActiveActors.Add(Id);
826  }
827  else
828  {
829  LM_LOG(Warning, "Actor %d could not be woken up, keeping sleep state", Id);
830  DormantActors.Add(Id);
831  }
832  }
833  DormantToActiveActors.Reset();
834 }
835 
837 {
838  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::CheckIfRebaseIsNeeded);
839  if(ActorsToConsider.Num() > 0)
840  {
841  UWorld* World = GetWorld();
842  UWorldComposition* WorldComposition = World->WorldComposition;
843  // TODO: consider multiple hero vehicles for rebasing
844  AActor* ActorToConsider = ActorsToConsider[0];
845  if( IsValid(ActorToConsider) )
846  {
847  FVector ActorLocation = ActorToConsider->GetActorLocation();
848  FIntVector ILocation = FIntVector(ActorLocation.X, ActorLocation.Y, ActorLocation.Z);
849  //WorldComposition->EvaluateWorldOriginLocation(ActorToConsider->GetActorLocation());
850  if (ActorLocation.SizeSquared() > FMath::Square(RebaseOriginDistance) )
851  {
852  // LM_LOG(Error, "Rebasing from %s to %s", *CurrentOriginInt.ToString(), *(ILocation + CurrentOriginInt).ToString());
853  // World->SetNewWorldOrigin(ILocation + CurrentOriginInt);
854  TileID TileId = GetTileID(CurrentOriginD + ActorLocation);
855  FVector NewOrigin = GetTileLocation(TileId);
856  World->SetNewWorldOrigin(FIntVector(NewOrigin));
857  }
858  }
859  }
860 }
861 
862 void ALargeMapManager::GetTilesToConsider(const AActor* ActorToConsider,
863  TSet<TileID>& OutTilesToConsider)
864 {
865  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::GetTilesToConsider);
866  check(ActorToConsider);
867  // World location
868  FDVector ActorLocation = CurrentOriginD + ActorToConsider->GetActorLocation();
869 
870  // Calculate Current Tile
871  FIntVector CurrentTile = GetTileVectorID(ActorLocation);
872 
873  // Calculate tile bounds
874  FDVector UpperPos = ActorLocation + FDVector(LayerStreamingDistance,LayerStreamingDistance,0);
875  FDVector LowerPos = ActorLocation + FDVector(-LayerStreamingDistance,-LayerStreamingDistance,0);
876  FIntVector UpperTileId = GetTileVectorID(UpperPos);
877  FIntVector LowerTileId = GetTileVectorID(LowerPos);
878  for (int Y = UpperTileId.Y; Y <= LowerTileId.Y; Y++)
879  {
880  for (int X = LowerTileId.X; X <= UpperTileId.X; X++)
881  {
882  // I don't check the bounds of the Tile map, if the Tile does not exist
883  // I just simply discard it
884  FIntVector TileToCheck = FIntVector(X, Y, 0);
885 
886  TileID TileID = GetTileID(TileToCheck);
887  FCarlaMapTile* Tile = MapTiles.Find(TileID);
888  if (!Tile)
889  {
890  // LM_LOG(Warning, "Requested tile %d, %d but tile was not found", TileToCheck.X, TileToCheck.Y);
891  continue; // Tile does not exist, discard
892  }
893 
894  OutTilesToConsider.Add(TileID);
895  }
896  }
897 }
898 
900  const TSet<TileID>& InTilesToConsider,
901  TSet<TileID>& OutTilesToBeVisible,
902  TSet<TileID>& OutTilesToHidde)
903 {
904  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::GetTilesThatNeedToChangeState);
905  OutTilesToBeVisible = InTilesToConsider.Difference(CurrentTilesLoaded);
906  OutTilesToHidde = CurrentTilesLoaded.Difference(InTilesToConsider);
907 }
908 
910  const TSet<TileID>& InTilesToUpdate,
911  bool InShouldBlockOnLoad,
912  bool InShouldBeLoaded,
913  bool InShouldBeVisible)
914 {
915  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::UpdateTileState);
916  UWorld* World = GetWorld();
917  UWorldComposition* WorldComposition = World->WorldComposition;
918 
919  // Gather all the locations of the levels to load
920  for (const TileID TileID : InTilesToUpdate)
921  {
922  FCarlaMapTile* CarlaTile = MapTiles.Find(TileID);
923  check(CarlaTile); // If an invalid ID reach here, we did something very wrong
924  ULevelStreamingDynamic* StreamingLevel = CarlaTile->StreamingLevel;
925  StreamingLevel->bShouldBlockOnLoad = InShouldBlockOnLoad;
926  StreamingLevel->SetShouldBeLoaded(InShouldBeLoaded);
927  StreamingLevel->SetShouldBeVisible(InShouldBeVisible);
928  }
929 }
930 
932  const TSet<TileID>& InTilesToBeVisible,
933  const TSet<TileID>& InTilesToHidde)
934 {
935  TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::UpdateCurrentTilesLoaded);
936  for (const TileID TileID : InTilesToHidde)
937  {
938  CurrentTilesLoaded.Remove(TileID);
939  }
940 
941  for (const TileID TileID : InTilesToBeVisible)
942  {
944  }
945 }
946 
948 {
949  int32 X = (int32)(TileID >> 32);
950  int32 Y = (int32)(TileID);
951  return FString::Printf(TEXT("Tile_%d_%d"), X, Y);
952 }
953 
955 {
956  int32 X = (int32)(TileID >> 32);
957  int32 Y = (int32)(TileID);
958  return FString::Printf(TEXT("%d_%d"), X, Y);
959 }
960 
962 {
963  FString FileContent = "";
964  FileContent += FString::Printf(TEXT("LargeMapManager state\n"));
965 
966  FileContent += FString::Printf(TEXT("Tile:\n"));
967  FileContent += FString::Printf(TEXT("ID\tName\tLocation\n"));
968  for (auto& It : MapTiles)
969  {
970  const FCarlaMapTile& Tile = It.Value;
971  FileContent += FString::Printf(TEXT(" %ld\t%s\t%s\n"), It.Key, *Tile.Name, *Tile.Location.ToString());
972  }
973  FileContent += FString::Printf(TEXT("\nNum generated tiles: %d\n"), MapTiles.Num());
974 
975  // Generate the map name with the assets folder name
976  TArray<FString> StringArray;
977  AssetsPath.ParseIntoArray(StringArray, TEXT("/"), false);
978 
979  FString FilePath = FPaths::ProjectSavedDir() + StringArray[StringArray.Num() - 1] + ".txt";
980  FFileHelper::SaveStringToFile(
981  FileContent,
982  *FilePath,
983  FFileHelper::EEncodingOptions::AutoDetect,
985  EFileWrite::FILEWRITE_Silent);
986 }
987 
989 {
990  UWorld* World = GetWorld();
991 
992  FDVector CurrentActorPosition;
993  if (ActorsToConsider.Num() > 0)
994  CurrentActorPosition = CurrentOriginD + ActorsToConsider[0]->GetActorLocation();
995 
996  const TArray<FLevelCollection>& WorldLevelCollections = World->GetLevelCollections();
997  const FLevelCollection* LevelCollection = World->GetActiveLevelCollection();
998  const TArray<ULevel*>& Levels = World->GetLevels();
999  const TArray<ULevelStreaming*>& StreamingLevels = World->GetStreamingLevels();
1000  ULevel* CurrentLevel = World->GetCurrentLevel();
1001 
1002  FString Output = "";
1003  Output += FString::Printf(TEXT("Num levels in world composition: %d\n"), World->WorldComposition->TilesStreaming.Num());
1004  Output += FString::Printf(TEXT("Num levels loaded: %d\n"), Levels.Num() );
1005  Output += FString::Printf(TEXT("Num tiles loaded: %d\n"), CurrentTilesLoaded.Num() );
1006  Output += FString::Printf(TEXT("Tiles loaded: [ "));
1007  for(TileID& TileId : CurrentTilesLoaded)
1008  {
1009  Output += FString::Printf(TEXT("%s, "), *TileIDToString(TileId));
1010  }
1011  Output += FString::Printf(TEXT("]\n"));
1012  GEngine->AddOnScreenDebugMessage(0, MsgTime, FColor::Cyan, Output);
1013 
1014  int LastMsgIndex = TilesDistMsgIndex;
1015  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White,
1016  FString::Printf(TEXT("\nActor Global Position: %s km"), *(FDVector(CurrentActorPosition) / (1000.0 * 100.0)).ToString()) );
1017 
1018  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White, TEXT("Closest tiles - Distance:"));
1019 
1020  for (const auto& TilePair : MapTiles)
1021  {
1022  const FCarlaMapTile& Tile = TilePair.Value;
1023  const ULevelStreaming* Level = Tile.StreamingLevel;
1024  FVector LevelLocation = Tile.Location;
1025  float Distance = FDVector::Dist(LevelLocation, CurrentActorPosition);
1026 
1027  FColor MsgColor = FColor::Green;
1028  if (Distance < (TileSide * 2.0f))
1029  {
1030  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, MsgColor,
1031  FString::Printf(TEXT("%s %.2f"), *Level->GetName(), Distance / (1000.0f * 100.0f)));
1032  }
1033  if (LastMsgIndex < MaxTilesDistMsgIndex) break;
1034  }
1035  LastMsgIndex = ClientLocMsgIndex;
1036  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White,
1037  FString::Printf(TEXT("\nOrigin: %s km"), *(FDVector(CurrentOriginInt) / (1000.0 * 100.0)).ToString()) );
1038  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White,
1039  FString::Printf(TEXT("Num active actors (%d)"), ActiveActors.Num()) );
1040  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White,
1041  FString::Printf(TEXT("Num dormant actors (%d)"), DormantActors.Num()));
1042  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White,
1043  FString::Printf(TEXT("Actors To Consider (%d)"), ActorsToConsider.Num()));
1044  for (const AActor* Actor : ActorsToConsider)
1045  {
1046  if (IsValid(Actor))
1047  {
1048  Output = "";
1049  float ToKm = 1000.0f * 100.0f;
1050  FVector TileActorLocation = Actor->GetActorLocation();
1051  FDVector ClientActorLocation = CurrentOriginD + FDVector(TileActorLocation);
1052 
1053  Output += FString::Printf(TEXT("Local Loc: %s meters\n"), *(TileActorLocation / ToKm).ToString());
1054  Output += FString::Printf(TEXT("Client Loc: %s km\n"), *(ClientActorLocation / ToKm).ToString());
1055  Output += "---------------";
1056  GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, PositonMsgColor, Output);
1057 
1058  if (LastMsgIndex > MaxClientLocMsgIndex) break;
1059  }
1060  }
1061 }
FCarlaActor * FindCarlaActor(FCarlaActor::IdType ActorId)
Find a Carla actor by id.
Definition: CarlaEpisode.h:152
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:251
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:246
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:134
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:34
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:290
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:29