CARLA
CarlaEpisode.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #include "Carla.h"
9 
12 #include <carla/rpc/String.h>
14 
15 #include "Carla/Sensor/Sensor.h"
21 
22 #include "Engine/StaticMeshActor.h"
23 #include "EngineUtils.h"
24 #include "GameFramework/SpectatorPawn.h"
25 #include "GenericPlatform/GenericPlatformProcess.h"
26 #include "Kismet/GameplayStatics.h"
27 #include "Misc/FileHelper.h"
28 #include "Misc/Paths.h"
29 
31 {
32  using TSS = ETrafficSignState;
33  switch (State)
34  {
35  case TSS::TrafficLightRed:
36  case TSS::TrafficLightYellow:
37  case TSS::TrafficLightGreen: return TEXT("traffic.traffic_light");
38  case TSS::SpeedLimit_30: return TEXT("traffic.speed_limit.30");
39  case TSS::SpeedLimit_40: return TEXT("traffic.speed_limit.40");
40  case TSS::SpeedLimit_50: return TEXT("traffic.speed_limit.50");
41  case TSS::SpeedLimit_60: return TEXT("traffic.speed_limit.60");
42  case TSS::SpeedLimit_90: return TEXT("traffic.speed_limit.90");
43  case TSS::SpeedLimit_100: return TEXT("traffic.speed_limit.100");
44  case TSS::SpeedLimit_120: return TEXT("traffic.speed_limit.120");
45  case TSS::SpeedLimit_130: return TEXT("traffic.speed_limit.130");
46  case TSS::StopSign: return TEXT("traffic.stop");
47  case TSS::YieldSign: return TEXT("traffic.yield");
48  default: return TEXT("traffic.unknown");
49  }
50 }
51 
52 UCarlaEpisode::UCarlaEpisode(const FObjectInitializer &ObjectInitializer)
53  : Super(ObjectInitializer),
54  Id(URandomEngine::GenerateRandomId())
55 {
56  ActorDispatcher = CreateDefaultSubobject<UActorDispatcher>(TEXT("ActorDispatcher"));
57 }
58 
59 bool UCarlaEpisode::LoadNewEpisode(const FString &MapString, bool ResetSettings)
60 {
61  bool bIsFileFound = false;
62 
63  FString FinalPath = MapString.IsEmpty() ? GetMapName() : MapString;
64  FinalPath += !MapString.EndsWith(".umap") ? ".umap" : "";
65 
66  if (MapString.StartsWith("/Game"))
67  {
68  // Some conversions...
69  FinalPath.RemoveFromStart(TEXT("/Game/"));
70  FinalPath = FPaths::ProjectContentDir() + FinalPath;
71  FinalPath = IFileManager::Get().ConvertToAbsolutePathForExternalAppForRead(*FinalPath);
72 
73  if (FPaths::FileExists(FinalPath)) {
74  bIsFileFound = true;
75  FinalPath = MapString;
76  }
77  }
78  else
79  {
80  if (MapString.Contains("/")) return false;
81 
82  // Find the full path under Carla
83  TArray<FString> TempStrArray, PathList;
84  IFileManager::Get().FindFilesRecursive(PathList, *FPaths::ProjectContentDir(), *FinalPath, true, false, false);
85  if (PathList.Num() > 0)
86  {
87  FinalPath = PathList[0];
88  FinalPath.ParseIntoArray(TempStrArray, TEXT("Content/"), true);
89  FinalPath = TempStrArray[1];
90  FinalPath.ParseIntoArray(TempStrArray, TEXT("."), true);
91  FinalPath = "/Game/" + TempStrArray[0];
92 
93  return LoadNewEpisode(FinalPath, ResetSettings);
94  }
95  }
96 
97  if (bIsFileFound)
98  {
99  UE_LOG(LogCarla, Warning, TEXT("Loading a new episode: %s"), *FinalPath);
100  UGameplayStatics::OpenLevel(GetWorld(), *FinalPath, true);
101  if (ResetSettings)
103  }
104  return bIsFileFound;
105 }
106 
107 static FString BuildRecastBuilderFile()
108 {
109  // Define filename with extension depending on if we are on Windows or not
110 #if PLATFORM_WINDOWS
111  const FString RecastToolName = "RecastBuilder.exe";
112 #else
113  const FString RecastToolName = "RecastBuilder";
114 #endif // PLATFORM_WINDOWS
115 
116  // Define path depending on the UE4 build type (Package or Editor)
117 #if UE_BUILD_SHIPPING
118  const FString AbsoluteRecastBuilderPath = FPaths::ConvertRelativePathToFull(
119  FPaths::RootDir() + "Tools/" + RecastToolName);
120 #else
121  const FString AbsoluteRecastBuilderPath = FPaths::ConvertRelativePathToFull(
122  FPaths::ProjectDir() + "../../Util/DockerUtils/dist/" + RecastToolName);
123 #endif
124  return AbsoluteRecastBuilderPath;
125 }
126 
128  const FString &OpenDriveString,
130 {
131  if (OpenDriveString.IsEmpty())
132  {
133  UE_LOG(LogCarla, Error, TEXT("The OpenDrive string is empty."));
134  return false;
135  }
136 
137  // Build the Map from the OpenDRIVE data
138  const auto CarlaMap = carla::opendrive::OpenDriveParser::Load(
139  carla::rpc::FromLongFString(OpenDriveString));
140 
141  // Check the Map is correclty generated
142  if (!CarlaMap.has_value())
143  {
144  UE_LOG(LogCarla, Error, TEXT("The OpenDrive string is invalid or not supported"));
145  return false;
146  }
147 
148  // Generate the OBJ (as string)
149  const auto RoadMesh = CarlaMap->GenerateMesh(Params.vertex_distance);
150  const auto CrosswalksMesh = CarlaMap->GetAllCrosswalkMesh();
151  const auto RecastOBJ = (RoadMesh + CrosswalksMesh).GenerateOBJForRecast();
152 
153  const FString AbsoluteOBJPath = FPaths::ConvertRelativePathToFull(
154  FPaths::ProjectContentDir() + "Carla/Maps/Nav/OpenDriveMap.obj");
155 
156  // Store the OBJ string to a file in order to that RecastBuilder can load it
157  FFileHelper::SaveStringToFile(
158  carla::rpc::ToLongFString(RecastOBJ),
159  *AbsoluteOBJPath,
160  FFileHelper::EEncodingOptions::ForceUTF8,
161  &IFileManager::Get());
162 
163  const FString AbsoluteXODRPath = FPaths::ConvertRelativePathToFull(
164  FPaths::ProjectContentDir() + "Carla/Maps/OpenDrive/OpenDriveMap.xodr");
165 
166  // Copy the OpenDrive as a file in the serverside
167  FFileHelper::SaveStringToFile(
168  OpenDriveString,
169  *AbsoluteXODRPath,
170  FFileHelper::EEncodingOptions::ForceUTF8,
171  &IFileManager::Get());
172 
173  if (!FPaths::FileExists(AbsoluteXODRPath))
174  {
175  UE_LOG(LogCarla, Error, TEXT("ERROR: XODR not copied!"));
176  return false;
177  }
178 
179  UCarlaGameInstance * GameInstance = UCarlaStatics::GetGameInstance(GetWorld());
180  if(GameInstance)
181  {
182  GameInstance->SetOpendriveGenerationParameters(Params);
183  }
184  else
185  {
186  carla::log_warning("Missing game instance");
187  }
188 
189  const FString AbsoluteRecastBuilderPath = BuildRecastBuilderFile();
190 
191  if (FPaths::FileExists(AbsoluteRecastBuilderPath) &&
193  {
194  /// @todo this can take too long to finish, clients need a method
195  /// to know if the navigation is available or not.
196  FPlatformProcess::CreateProc(
197  *AbsoluteRecastBuilderPath, *AbsoluteOBJPath,
198  true, true, true, nullptr, 0, nullptr, nullptr);
199  }
200  else
201  {
202  UE_LOG(LogCarla, Warning, TEXT("'RecastBuilder' not present under '%s', "
203  "the binaries for pedestrian navigation will not be created."),
204  *AbsoluteRecastBuilderPath);
205  }
206 
207  return true;
208 }
209 
211 {
212  EpisodeSettings = Settings;
214  {
215  UE_LOG(LogCarla, Warning, TEXT("Setting ActorActiveDistance is smaller that TileStreamingDistance, TileStreamingDistance will be increased"));
217  }
219 }
220 
222 {
224 
225  return GM->GetSpawnPointsTransforms();
226 }
227 
229 {
231  if (CarlaActor)
232  {
233  Actor = CarlaActor->GetActorInfo()->SerializedData;
234  auto ParentId = CarlaActor->GetParent();
235  if (ParentId)
236  {
237  Actor.parent_id = ParentId;
238  }
239  }
240  else
241  {
242  UE_LOG(LogCarla, Warning, TEXT("Trying to serialize invalid actor"));
243  }
244  return Actor;
245 }
246 
247 static FString GetRelevantTagAsString(const TSet<crp::CityObjectLabel> &SemanticTags);
248 
250 {
251  FCarlaActor* CarlaActor = FindCarlaActor(Actor);
252  if (CarlaActor)
253  {
254  return SerializeActor(CarlaActor);
255  }
256  else
257  {
258  carla::rpc::Actor SerializedActor;
259  SerializedActor.id = 0u;
261  TSet<crp::CityObjectLabel> SemanticTags;
262  ATagger::GetTagsOfTaggedActor(*Actor, SemanticTags);
263  FActorDescription Description;
264  Description.Id = TEXT("static.") + GetRelevantTagAsString(SemanticTags);
265  SerializedActor.description = Description;
266  SerializedActor.semantic_tags.reserve(SemanticTags.Num());
267  for (auto &&Tag : SemanticTags)
268  {
269  using tag_t = decltype(SerializedActor.semantic_tags)::value_type;
270  SerializedActor.semantic_tags.emplace_back(static_cast<tag_t>(Tag));
271  }
272  return SerializedActor;
273  }
274 }
275 
277  AActor *Child,
278  AActor *Parent,
279  EAttachmentType InAttachmentType)
280 {
281  Child->AddActorWorldOffset(FVector(CurrentMapOrigin));
282 
283  UActorAttacher::AttachActors(Child, Parent, InAttachmentType);
284 
285  // recorder event
286  if (Recorder->IsEnabled())
287  {
288  CarlaRecorderEventParent RecEvent
289  {
290  FindCarlaActor(Child)->GetActorId(),
291  FindCarlaActor(Parent)->GetActorId()
292  };
293  Recorder->AddEvent(std::move(RecEvent));
294  }
295 }
296 
298 {
299  auto World = GetWorld();
300  check(World != nullptr);
301  auto PlayerController = UGameplayStatics::GetPlayerController(World, 0);
302  if (PlayerController == nullptr)
303  {
304  UE_LOG(LogCarla, Error, TEXT("Can't find player controller!"));
305  return;
306  }
307  Spectator = PlayerController->GetPawn();
308  if (Spectator != nullptr)
309  {
310  FActorDescription Description;
311  Description.Id = TEXT("spectator");
312  Description.Class = Spectator->GetClass();
313  ActorDispatcher->RegisterActor(*Spectator, Description);
314  }
315  else
316  {
317  UE_LOG(LogCarla, Error, TEXT("Can't find spectator!"));
318  }
319 
320  for (TActorIterator<ATrafficSignBase> It(World); It; ++It)
321  {
322  ATrafficSignBase *Actor = *It;
323  check(Actor != nullptr);
324  FActorDescription Description;
325  Description.Id = UCarlaEpisode_GetTrafficSignId(Actor->GetTrafficSignState());
326  Description.Class = Actor->GetClass();
327  ActorDispatcher->RegisterActor(*Actor, Description);
328  }
329 
330  // get the definition id for static.prop.mesh
331  auto Definitions = GetActorDefinitions();
332  uint32 StaticMeshUId = 0;
333  for (auto& Definition : Definitions)
334  {
335  if (Definition.Id == "static.prop.mesh")
336  {
337  StaticMeshUId = Definition.UId;
338  break;
339  }
340  }
341 
342  for (TActorIterator<AStaticMeshActor> It(World); It; ++It)
343  {
344  auto Actor = *It;
345  check(Actor != nullptr);
346  auto MeshComponent = Actor->GetStaticMeshComponent();
347  check(MeshComponent != nullptr);
348  if (MeshComponent->Mobility == EComponentMobility::Movable)
349  {
350  FActorDescription Description;
351  Description.Id = TEXT("static.prop.mesh");
352  Description.UId = StaticMeshUId;
353  Description.Class = Actor->GetClass();
354  Description.Variations.Add("mesh_path",
356  MeshComponent->GetStaticMesh()->GetPathName()});
357  Description.Variations.Add("mass",
359  FString::SanitizeFloat(MeshComponent->GetMass())});
360  ActorDispatcher->RegisterActor(*Actor, Description);
361  }
362  }
363 }
364 
366 {
367  // stop recorder and replayer
368  if (Recorder)
369  {
370  Recorder->Stop();
371  if (Recorder->GetReplayer()->IsEnabled())
372  {
373  Recorder->GetReplayer()->Stop();
374  }
375  }
376 }
377 
378 std::string UCarlaEpisode::StartRecorder(std::string Name, bool AdditionalData)
379 {
380  std::string result;
381 
382  if (Recorder)
383  {
384  result = Recorder->Start(Name, MapName, AdditionalData);
385  }
386  else
387  {
388  result = "Recorder is not ready";
389  }
390 
391  return result;
392 }
393 
394 TPair<EActorSpawnResultStatus, FCarlaActor*> UCarlaEpisode::SpawnActorWithInfo(
395  const FTransform &Transform,
396  FActorDescription thisActorDescription,
397  FCarlaActor::IdType DesiredId)
398 {
399  ALargeMapManager* LargeMap = UCarlaStatics::GetLargeMapManager(GetWorld());
400  FTransform LocalTransform = Transform;
401  if(LargeMap)
402  {
403  LocalTransform = LargeMap->GlobalToLocalTransform(LocalTransform);
404  }
405 
406  // NewTransform.AddToTranslation(-1.0f * FVector(CurrentMapOrigin));
407  auto result = ActorDispatcher->SpawnActor(LocalTransform, thisActorDescription, DesiredId);
408  if (Recorder->IsEnabled())
409  {
410  if (result.Key == EActorSpawnResultStatus::Success)
411  {
413  result.Value->GetActorId(),
414  static_cast<uint8_t>(result.Value->GetActorType()),
415  Transform,
416  std::move(thisActorDescription)
417  );
418  }
419  }
420 
421  return result;
422 }
FCarlaActor * FindCarlaActor(FCarlaActor::IdType ActorId)
Find a Carla actor by id.
Definition: CarlaEpisode.h:152
bool IsEnabled(void)
Definition: CarlaReplayer.h:65
Seting for map generation from opendrive without additional geometry.
bool LoadNewOpendriveEpisode(const FString &OpenDriveString, const carla::rpc::OpendriveGenerationParameters &Params)
Load a new map generating the mesh from OpenDRIVE data and start a new episode.
std::string Start(std::string Name, FString MapName, bool AdditionalData=false)
TPair< EActorSpawnResultStatus, FCarlaActor * > SpawnActor(const FTransform &Transform, FActorDescription ActorDescription, FCarlaActor::IdType DesiredId=0)
Spawns an actor based on ActorDescription at Transform.
FString MapName
Definition: CarlaEpisode.h:324
UActorDispatcher * ActorDispatcher
Definition: CarlaEpisode.h:330
FCarlaActor * RegisterActor(AActor &Actor, FActorDescription ActorDescription, FActorRegistry::IdType DesiredId=0)
Register an actor that was not created using "SpawnActor" function but that should be kept in the reg...
ETrafficSignState
static void AttachActors(AActor *Child, AActor *Parent, EAttachmentType AttachmentType)
EAttachmentType
Definition: ActorAttacher.h:20
The game instance contains elements that must be kept alive in between levels.
carla::rpc::Actor SerializeActor(FCarlaActor *CarlaActor) const
Create a serializable object describing the actor.
static FOnEpisodeSettingsChange OnEpisodeSettingsChange
TArray< FTransform > GetRecommendedSpawnPoints() const
Return the list of recommended spawn points for vehicles.
void AddEvent(const CarlaRecorderEventAdd &Event)
static void GetTagsOfTaggedActor(const AActor &Actor, TSet< crp::CityObjectLabel > &Tags)
Retrieve the tags of an already tagged actor.
Definition: Tagger.cpp:110
uint32 IdType
Definition: CarlaActor.h:27
FIntVector CurrentMapOrigin
Definition: CarlaEpisode.h:342
CarlaReplayer * GetReplayer(void)
static FString BuildRecastBuilderFile()
static T Get(carla::rpc::Response< T > &response)
ActorDescription description
Definition: rpc/Actor.h:29
static ACarlaGameModeBase * GetGameMode(const UObject *WorldContextObject)
Definition: CarlaStatics.h:58
static boost::optional< road::Map > Load(const std::string &opendrive)
const FString & GetMapName() const
Return the name of the map loaded in this episode.
Definition: CarlaEpisode.h:89
geom::BoundingBox bounding_box
Definition: rpc/Actor.h:31
void CreateRecorderEventAdd(uint32_t DatabaseId, uint8_t Type, const FTransform &Transform, FActorDescription ActorDescription)
TMap< FString, FActorAttribute > Variations
User selected variations of the actor.
std::string StartRecorder(std::string name, bool AdditionalData)
static FString GetRelevantTagAsString(const TSet< crp::CityObjectLabel > &SemanticTags)
static UCarlaGameInstance * GetGameInstance(const UObject *WorldContextObject)
Definition: CarlaStatics.h:63
carla::SharedPtr< cc::Actor > Actor
crp::Actor SerializedData
Definition: ActorInfo.h:32
TPair< EActorSpawnResultStatus, FCarlaActor * > SpawnActorWithInfo(const FTransform &Transform, FActorDescription thisActorDescription, FCarlaActor::IdType DesiredId=0)
Spawns an actor based on ActorDescription at Transform.
bool LoadNewEpisode(const FString &MapString, bool ResetSettings=true)
Load a new map and start a new episode.
const TArray< FActorDefinition > & GetActorDefinitions() const
Return the list of actor definitions that are available to be spawned this episode.
Definition: CarlaEpisode.h:103
TSubclassOf< AActor > Class
Class of the actor to be spawned.
static FBoundingBox GetActorBoundingBox(const AActor *Actor, uint8 InTagQueried=0xFF)
Compute the bounding box of the given Carla actor.
A description of a Carla Actor with all its variation.
bool IsEnabled(void)
Definition: CarlaRecorder.h:77
static void log_warning(Args &&... args)
Definition: Logging.h:96
const FActorInfo * GetActorInfo() const
Definition: CarlaActor.h:100
static FString UCarlaEpisode_GetTrafficSignId(ETrafficSignState State)
APawn * Spectator
Definition: CarlaEpisode.h:333
std::vector< uint8_t > semantic_tags
Definition: rpc/Actor.h:33
UCarlaEpisode(const FObjectInitializer &ObjectInitializer)
void AttachActors(AActor *Child, AActor *Parent, EAttachmentType InAttachmentType=EAttachmentType::Rigid)
Attach Child to Parent.
FTransform GlobalToLocalTransform(const FTransform &InTransform) const
ACarlaRecorder * Recorder
Definition: CarlaEpisode.h:338
static ALargeMapManager * GetLargeMapManager(const UObject *WorldContextObject)
Definition: CarlaStatics.h:100
uint32 UId
UId of the definition in which this description was based.
Base class for the CARLA Game Mode.
void InitializeAtBeginPlay()
FEpisodeSettings EpisodeSettings
Definition: CarlaEpisode.h:327
void Stop(bool KeepActors=false)
ActorId parent_id
Definition: rpc/Actor.h:27
IdType GetActorId() const
Definition: CarlaActor.h:80
ETrafficSignState GetTrafficSignState() const
void SetOpendriveGenerationParameters(const carla::rpc::OpendriveGenerationParameters &Parameters)
An actor attribute, may be an intrinsic (non-modifiable) attribute of the actor or an user-defined ac...
geom::Transform Transform
Definition: rpc/Transform.h:16
const TArray< FTransform > & GetSpawnPointsTransforms() const
A view over an actor and its properties.
Definition: CarlaActor.h:23
void ApplySettings(const FEpisodeSettings &Settings)
IdType GetParent() const
Definition: CarlaActor.h:120