CARLA
 
载入中...
搜索中...
未找到
CarlaEpisode.cpp
浏览该文件的文档.
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" // 包含主Carla头文件,提供Carla仿真框架的核心功能
8#include "Carla/Game/CarlaEpisode.h" // 包含Carla游戏环节的头文件,表示仿真会话
9#include "Carla/Game/CarlaStatics.h" // 包含Carla静态变量和函数的头文件,提供全局访问点
10
11#include <compiler/disable-ue4-macros.h> // 禁用Unreal Engine的宏,防止与Carla代码冲突
12#include <carla/opendrive/OpenDriveParser.h> // 包含Carla OpenDrive解析器的头文件,用于解析OpenDrive文件
13#include <carla/rpc/String.h> // 包含Carla RPC字符串类型的头文件,用于网络通信
14#include <compiler/enable-ue4-macros.h> // 启用Unreal Engine的宏
15
16#include "Carla/Sensor/Sensor.h" // 包含Carla传感器的头文件,表示仿真中的传感器
17#include "Carla/Util/BoundingBoxCalculator.h" // 包含边界框计算器的头文件,用于计算Actor的边界框
18#include "Carla/Util/RandomEngine.h" // 包含随机引擎的头文件,提供随机数生成功能
19#include "Carla/Vehicle/VehicleSpawnPoint.h" // 包含车辆生成点的头文件,表示车辆在地图上的生成位置
20#include "Carla/Game/CarlaStatics.h" // 重复包含,可能是为了强调其重要性
21#include "Carla/Game/CarlaStaticDelegates.h" // 包含Carla静态委托的头文件,用于注册和管理事件
22#include "Carla/MapGen/LargeMapManager.h" // 包含大地图管理器的头文件,管理大型开放世界地图
23
24#include "Engine/StaticMeshActor.h" // 包含Unreal Engine静态网格Actor的头文件,表示3D模型
25#include "EngineUtils.h" // 包含Unreal Engine工具的头文件,提供通用工具函数
26#include "GameFramework/SpectatorPawn.h" // 包含观察者Pawn的头文件,表示玩家的观察角色
27#include "GenericPlatform/GenericPlatformProcess.h" // 包含通用平台进程的头文件,提供跨平台进程功能
28#include "Kismet/GameplayStatics.h" // 包含Kismet游戏逻辑静态函数的头文件,提供蓝图可用的静态函数
29#include "Materials/MaterialParameterCollection.h" // 包含材质参数集合的头文件,用于材质参数管理
30#include "Materials/MaterialParameterCollectionInstance.h" // 包含材质参数集合实例的头文件,用于材质参数的实例化
31#include "Misc/FileHelper.h" // 包含文件帮助函数的头文件,提供文件操作功能
32#include "Misc/Paths.h" // 包含路径管理的头文件,提供路径相关功能
33
34// 静态函数,根据交通标志状态返回对应的标识符
36{
37 using TSS = ETrafficSignState; // 使用别名TSS简化ETrafficSignState的调用
38 switch (State) // 根据交通标志状态进行switch判断
39 {
40 case TSS::TrafficLightRed: // 红灯状态
41 case TSS::TrafficLightYellow: // 黄灯状态
42 case TSS::TrafficLightGreen: // 绿灯状态
43 return TEXT("traffic.traffic_light"); // 返回交通灯的标识符
44 case TSS::SpeedLimit_30: // 速度限制30标识
45 return TEXT("traffic.speed_limit.30");
46 case TSS::SpeedLimit_40: // 速度限制40标识
47 return TEXT("traffic.speed_limit.40");
48 case TSS::SpeedLimit_50: // 速度限制50标识
49 return TEXT("traffic.speed_limit.50");
50 case TSS::SpeedLimit_60: // 速度限制60标识
51 return TEXT("traffic.speed_limit.60");
52 case TSS::SpeedLimit_90: // 速度限制90标识
53 return TEXT("traffic.speed_limit.90");
54 case TSS::SpeedLimit_100: // 速度限制100标识
55 return TEXT("traffic.speed_limit.100");
56 case TSS::SpeedLimit_120: // 速度限制120标识
57 return TEXT("traffic.speed_limit.120");
58 case TSS::SpeedLimit_130: // 速度限制130标识
59 return TEXT("traffic.speed_limit.130");
60 case TSS::StopSign: // 停车标识
61 return TEXT("traffic.stop");
62 case TSS::YieldSign: // 让路标识
63 return TEXT("traffic.yield");
64 default: // 其他未知状态
65 return TEXT("traffic.unknown"); // 返回未知交通标志的标识符
66 }
67}
68
69
70UCarlaEpisode::UCarlaEpisode(const FObjectInitializer &ObjectInitializer)
71 : Super(ObjectInitializer),
72 Id(URandomEngine::GenerateRandomId())
73{
74 ActorDispatcher = CreateDefaultSubobject<UActorDispatcher>(TEXT("ActorDispatcher"));
75 FrameData.SetEpisode(this);
76}
77
78bool UCarlaEpisode::LoadNewEpisode(const FString &MapString, bool ResetSettings)
79{
80 bool bIsFileFound = false;
81
82 FString FinalPath = MapString.IsEmpty() ? GetMapName() : MapString;
83 FinalPath += !MapString.EndsWith(".umap") ? ".umap" : "";
84
85 if (MapString.StartsWith("/Game"))
86 {
87 // 一些转化...
88 FinalPath.RemoveFromStart(TEXT("/Game/"));
89 FinalPath = FPaths::ProjectContentDir() + FinalPath;
90 FinalPath = IFileManager::Get().ConvertToAbsolutePathForExternalAppForRead(*FinalPath);
91
92 if (FPaths::FileExists(FinalPath)) {
93 bIsFileFound = true;
94 FinalPath = MapString;
95 }
96 }
97 else
98 {
99 if (MapString.Contains("/")) return false;
100
101 // 在 Carla 下查找完整路径
102 TArray<FString> TempStrArray, PathList;
103 IFileManager::Get().FindFilesRecursive(PathList, *FPaths::ProjectContentDir(), *FinalPath, true, false, false);
104 if (PathList.Num() > 0)
105 {
106 FinalPath = PathList[0];
107 FinalPath.ParseIntoArray(TempStrArray, TEXT("Content/"), true);
108 FinalPath = TempStrArray[1];
109 FinalPath.ParseIntoArray(TempStrArray, TEXT("."), true);
110 FinalPath = "/Game/" + TempStrArray[0];
111
112 return LoadNewEpisode(FinalPath, ResetSettings);
113 }
114 }
115
116 if (bIsFileFound)
117 {
118 UE_LOG(LogCarla, Warning, TEXT("Loading a new episode: %s"), *FinalPath);
119 UGameplayStatics::OpenLevel(GetWorld(), *FinalPath, true);
120 if (ResetSettings)
121 ApplySettings(FEpisodeSettings{});
122
123 // 向所有辅助服务器发送 'LOAD_MAP' 命令(如果有)
124 if (bIsPrimaryServer)
125 {
126 UCarlaGameInstance *GameInstance = UCarlaStatics::GetGameInstance(GetWorld());
127 if(GameInstance)
128 {
129 FCarlaEngine *CarlaEngine = GameInstance->GetCarlaEngine();
130 auto SecondaryServer = CarlaEngine->GetSecondaryServer();
131 if (SecondaryServer->HasClientsConnected())
132 {
133 SecondaryServer->GetCommander().SendLoadMap(std::string(TCHAR_TO_UTF8(*FinalPath)));
134 }
135 }
136 }
137 }
138 return bIsFileFound;
139}
140
142{
143 // 定义带扩展名的文件名,具体取决于我们是否在 Windows 上
144#if PLATFORM_WINDOWS
145 const FString RecastToolName = "RecastBuilder.exe";
146#else
147 const FString RecastToolName = "RecastBuilder";
148#endif // PLATFORM_WINDOWS
149
150 // 根据 UE4 构建类型(包或编辑器)定义路径
151#if UE_BUILD_SHIPPING
152 const FString AbsoluteRecastBuilderPath = FPaths::ConvertRelativePathToFull(
153 FPaths::RootDir() + "Tools/" + RecastToolName);
154#else
155 const FString AbsoluteRecastBuilderPath = FPaths::ConvertRelativePathToFull(
156 FPaths::ProjectDir() + "../../Util/DockerUtils/dist/" + RecastToolName);
157#endif
158 return AbsoluteRecastBuilderPath;
159}
160
161bool UCarlaEpisode::LoadNewOpendriveEpisode(
162 const FString &OpenDriveString,
164{
165 if (OpenDriveString.IsEmpty())
166 {
167 UE_LOG(LogCarla, Error, TEXT("The OpenDrive string is empty."));
168 return false;
169 }
170
171 // 根据 OpenDRIVE 数据构建地图
172 const auto CarlaMap = carla::opendrive::OpenDriveParser::Load(
173 carla::rpc::FromLongFString(OpenDriveString));
174
175 // 检查地图是否正确生成
176 if (!CarlaMap.has_value())
177 {
178 UE_LOG(LogCarla, Error, TEXT("The OpenDrive string is invalid or not supported"));
179 return false;
180 }
181
182 // 生成 OBJ(作为字符串)
183 const auto RoadMesh = CarlaMap->GenerateMesh(Params.vertex_distance);
184 const auto CrosswalksMesh = CarlaMap->GetAllCrosswalkMesh();
185 const auto RecastOBJ = (RoadMesh + CrosswalksMesh).GenerateOBJForRecast();
186
187 const FString AbsoluteOBJPath = FPaths::ConvertRelativePathToFull(
188 FPaths::ProjectContentDir() + "Carla/Maps/Nav/OpenDriveMap.obj");
189
190 // 将 OBJ 字符串存储到文件中,以便 RecastBuilder 可以加载它
191 FFileHelper::SaveStringToFile(
192 carla::rpc::ToLongFString(RecastOBJ),
193 *AbsoluteOBJPath,
194 FFileHelper::EEncodingOptions::ForceUTF8,
195 &IFileManager::Get());
196
197 const FString AbsoluteXODRPath = FPaths::ConvertRelativePathToFull(
198 FPaths::ProjectContentDir() + "Carla/Maps/OpenDrive/OpenDriveMap.xodr");
199
200 // 将 OpenDrive 作为文件复制到服务器端
201 FFileHelper::SaveStringToFile(
202 OpenDriveString,
203 *AbsoluteXODRPath,
204 FFileHelper::EEncodingOptions::ForceUTF8,
205 &IFileManager::Get());
206
207 if (!FPaths::FileExists(AbsoluteXODRPath))
208 {
209 UE_LOG(LogCarla, Error, TEXT("ERROR: XODR not copied!"));
210 return false;
211 }
212
213 UCarlaGameInstance * GameInstance = UCarlaStatics::GetGameInstance(GetWorld());
214 if(GameInstance)
215 {
216 GameInstance->SetOpendriveGenerationParameters(Params);
217 }
218 else
219 {
220 carla::log_warning("Missing game instance");
221 }
222
223 const FString AbsoluteRecastBuilderPath = BuildRecastBuilderFile();
224
225 if (FPaths::FileExists(AbsoluteRecastBuilderPath) &&
227 {
228 /// @todo 这可能需要很长时间才能完成,客户端需要一个方法
229 /// 了解导航是否可用。
230 FPlatformProcess::CreateProc(
231 *AbsoluteRecastBuilderPath, *AbsoluteOBJPath,
232 true, true, true, nullptr, 0, nullptr, nullptr);
233 }
234 else
235 {
236 UE_LOG(LogCarla, Warning, TEXT("'RecastBuilder' not present under '%s', "
237 "the binaries for pedestrian navigation will not be created."),
238 *AbsoluteRecastBuilderPath);
239 }
240
241 return true;
242}
243
244void UCarlaEpisode::ApplySettings(const FEpisodeSettings &Settings)
245{
246 EpisodeSettings = Settings;
247 if(EpisodeSettings.ActorActiveDistance > EpisodeSettings.TileStreamingDistance)
248 {
249 UE_LOG(LogCarla, Warning, TEXT("Setting ActorActiveDistance is smaller that TileStreamingDistance, TileStreamingDistance will be increased"));
250 EpisodeSettings.TileStreamingDistance = EpisodeSettings.ActorActiveDistance;
251 }
252 FCarlaStaticDelegates::OnEpisodeSettingsChange.Broadcast(EpisodeSettings);
253}
254
255TArray<FTransform> UCarlaEpisode::GetRecommendedSpawnPoints() const
256{
258
259 return GM->GetSpawnPointsTransforms();
260}
261
262carla::rpc::Actor UCarlaEpisode::SerializeActor(FCarlaActor *CarlaActor) const
263{
265 if (CarlaActor)
266 {
267 Actor = CarlaActor->GetActorInfo()->SerializedData;
268 auto ParentId = CarlaActor->GetParent();
269 if (ParentId)
270 {
271 Actor.parent_id = ParentId;
272 }
273 }
274 else
275 {
276 UE_LOG(LogCarla, Warning, TEXT("Trying to serialize invalid actor"));
277 }
278 return Actor;
279}
280
281carla::rpc::Actor UCarlaEpisode::SerializeActor(AActor* Actor) const
282{
283 FCarlaActor* CarlaActor = FindCarlaActor(Actor);
284 if (CarlaActor)
285 {
286 return SerializeActor(CarlaActor);
287 }
288 else
289 {
290 carla::rpc::Actor SerializedActor;
291 SerializedActor.id = 0u;
293 TSet<crp::CityObjectLabel> SemanticTags;
294 ATagger::GetTagsOfTaggedActor(*Actor, SemanticTags);
295 FActorDescription Description;
296 Description.Id = TEXT("static.") + CarlaGetRelevantTagAsString(SemanticTags);
297 SerializedActor.description = Description;
298 SerializedActor.semantic_tags.reserve(SemanticTags.Num());
299 for (auto &&Tag : SemanticTags)
300 {
301 using tag_t = decltype(SerializedActor.semantic_tags)::value_type;
302 SerializedActor.semantic_tags.emplace_back(static_cast<tag_t>(Tag));
303 }
304 return SerializedActor;
305 }
306}
307
308void UCarlaEpisode::AttachActors(
309 AActor *Child,
310 AActor *Parent,
311 EAttachmentType InAttachmentType,
312 const FString& SocketName)
313{
314 Child->AddActorWorldOffset(FVector(CurrentMapOrigin));
315
316 UActorAttacher::AttachActors(Child, Parent, InAttachmentType, SocketName);
317
318 if (bIsPrimaryServer)
319 {
320 GetFrameData().AddEvent(
322 FindCarlaActor(Child)->GetActorId(),
323 FindCarlaActor(Parent)->GetActorId()});
324 }
325 // 记录器事件
326 if (Recorder->IsEnabled())
327 {
329 {
330 FindCarlaActor(Child)->GetActorId(),
331 FindCarlaActor(Parent)->GetActorId()
332 };
333 Recorder->AddEvent(std::move(RecEvent));
334 }
335}
336
337void UCarlaEpisode::InitializeAtBeginPlay()
338{
339 auto World = GetWorld();
340 check(World != nullptr);
341 auto PlayerController = UGameplayStatics::GetPlayerController(World, 0);
342 if (PlayerController == nullptr)
343 {
344 UE_LOG(LogCarla, Error, TEXT("Can't find player controller!"));
345 return;
346 }
347 Spectator = PlayerController->GetPawn();
348 if (Spectator != nullptr)
349 {
350 FActorDescription Description;
351 Description.Id = TEXT("spectator");
352 Description.Class = Spectator->GetClass();
353 ActorDispatcher->RegisterActor(*Spectator, Description);
354 }
355 else
356 {
357 UE_LOG(LogCarla, Error, TEXT("Can't find spectator!"));
358 }
359
360 // 材质参数集合
361 UMaterialParameterCollection *Collection = LoadObject<UMaterialParameterCollection>(nullptr, TEXT("/Game/Carla/Blueprints/Game/CarlaParameters.CarlaParameters"), nullptr, LOAD_None, nullptr);
362 if (Collection != nullptr)
363 {
364 MaterialParameters = World->GetParameterCollectionInstance(Collection);
365 if (MaterialParameters == nullptr)
366 {
367 UE_LOG(LogCarla, Error, TEXT("Can't find CarlaParameters instance!"));
368 }
369 }
370 else
371 {
372 UE_LOG(LogCarla, Error, TEXT("Can't find CarlaParameters asset!"));
373 }
374
375 for (TActorIterator<ATrafficSignBase> It(World); It; ++It)
376 {
377 ATrafficSignBase *Actor = *It;
378 check(Actor != nullptr);
379 FActorDescription Description;
380 Description.Id = UCarlaEpisode_GetTrafficSignId(Actor->GetTrafficSignState());
381 Description.Class = Actor->GetClass();
382 ActorDispatcher->RegisterActor(*Actor, Description);
383 }
384
385 // 获取 static.prop.mesh 的定义 ID
386 auto Definitions = GetActorDefinitions();
387 uint32 StaticMeshUId = 0;
388 for (auto& Definition : Definitions)
389 {
390 if (Definition.Id == "static.prop.mesh")
391 {
392 StaticMeshUId = Definition.UId;
393 break;
394 }
395 }
396
397 for (TActorIterator<AStaticMeshActor> It(World); It; ++It)
398 {
399 auto Actor = *It;
400 check(Actor != nullptr);
401 auto MeshComponent = Actor->GetStaticMeshComponent();
402 check(MeshComponent != nullptr);
403 if (MeshComponent->Mobility == EComponentMobility::Movable)
404 {
405 FActorDescription Description;
406 Description.Id = TEXT("static.prop.mesh");
407 Description.UId = StaticMeshUId;
408 Description.Class = Actor->GetClass();
409 Description.Variations.Add("mesh_path",
411 MeshComponent->GetStaticMesh()->GetPathName()});
412 Description.Variations.Add("mass",
414 FString::SanitizeFloat(MeshComponent->GetMass())});
415 ActorDispatcher->RegisterActor(*Actor, Description);
416 }
417 }
418}
419
420void UCarlaEpisode::EndPlay(void)
421{
422 // 停止录制器和播放器
423 if (Recorder)
424 {
425 Recorder->Stop();
426 if (Recorder->GetReplayer()->IsEnabled())
427 {
428 Recorder->GetReplayer()->Stop();
429 }
430 }
431}
432
433std::string UCarlaEpisode::StartRecorder(std::string Name, bool AdditionalData)
434{
435 std::string result;
436
437 if (Recorder)
438 {
439 result = Recorder->Start(Name, MapName, AdditionalData);
440 }
441 else
442 {
443 result = "Recorder is not ready";
444 }
445
446 return result;
447}
448
449TPair<EActorSpawnResultStatus, FCarlaActor*> UCarlaEpisode::SpawnActorWithInfo(
450 const FTransform &Transform,
451 FActorDescription thisActorDescription,
452 FCarlaActor::IdType DesiredId)
453{
455 FTransform LocalTransform = Transform;
456 if(LargeMap)
457 {
458 LocalTransform = LargeMap->GlobalToLocalTransform(LocalTransform);
459 }
460
461 // NewTransform.AddToTranslation(-1.0f * FVector(CurrentMapOrigin));
462 auto result = ActorDispatcher->SpawnActor(LocalTransform, thisActorDescription, DesiredId);
463 if (result.Key == EActorSpawnResultStatus::Success && bIsPrimaryServer)
464 {
465 if (Recorder->IsEnabled())
466 {
467 Recorder->CreateRecorderEventAdd(
468 result.Value->GetActorId(),
469 static_cast<uint8_t>(result.Value->GetActorType()),
470 Transform,
471 thisActorDescription
472 );
473 }
474 if (bIsPrimaryServer)
475 {
476 GetFrameData().CreateRecorderEventAdd(
477 result.Value->GetActorId(),
478 static_cast<uint8_t>(result.Value->GetActorType()),
479 Transform,
480 std::move(thisActorDescription));
481 }
482 }
483
484 return result;
485}
EAttachmentType
UE_LOG(LogCarla, Log, TEXT("UActorDispatcher::Destroying actor: '%s' %x"), *Id, Actor)
FString CarlaGetRelevantTagAsString(const TSet< crp::CityObjectLabel > &SemanticTags)
const FCarlaActor * FindCarlaActor(const AActor *Actor) const
TSharedPtr< const FActorInfo > carla::rpc::ActorState UWorld * World
TSharedPtr< const FActorInfo > carla::rpc::ActorState UWorld Actor
static FString UCarlaEpisode_GetTrafficSignId(ETrafficSignState State)
static FString BuildRecastBuilderFile()
ETrafficSignState
CARLA Game Mode 的基类。
const TArray< FTransform > & GetSpawnPointsTransforms() const
FTransform GlobalToLocalTransform(const FTransform &InTransform) const
static void GetTagsOfTaggedActor(const AActor &Actor, TSet< crp::CityObjectLabel > &Tags)
检索已标记的角色的标记。CityObjectLabel::None 为 未添加到数组中。
Definition Tagger.cpp:243
查看一个参与者和它的属性
Definition CarlaActor.h:23
IdType GetParent() const
Definition CarlaActor.h:99
uint32 IdType
Definition CarlaActor.h:25
const FActorInfo * GetActorInfo() const
Definition CarlaActor.h:83
IdType GetActorId() const
Definition CarlaActor.h:67
std::shared_ptr< carla::multigpu::Router > GetSecondaryServer()
Definition CarlaEngine.h:97
static FOnEpisodeSettingsChange OnEpisodeSettingsChange
static void AttachActors(AActor *Child, AActor *Parent, EAttachmentType AttachmentType, const FString &SocketName="")
static FBoundingBox GetActorBoundingBox(const AActor *Actor, uint8 InTagQueried=0xFF)
计算给定 Carla actor 的边界框。
FCarlaEngine * GetCarlaEngine()
void SetOpendriveGenerationParameters(const carla::rpc::OpendriveGenerationParameters &Parameters)
static UCarlaGameInstance * GetGameInstance(const UObject *WorldContextObject)
static ALargeMapManager * GetLargeMapManager(const UObject *WorldContextObject)
static ACarlaGameModeBase * GetGameMode(const UObject *WorldContextObject)
static boost::optional< road::Map > Load(const std::string &opendrive)
std::vector< uint8_t > semantic_tags
Definition rpc/Actor.h:44
ActorDescription description
Definition rpc/Actor.h:38
geom::BoundingBox bounding_box
Definition rpc/Actor.h:41
geom::Transform Transform
static void log_warning(Args &&... args)
Definition Logging.h:101
An actor attribute, may be an intrinsic (non-modifiable) attribute of the actor or an user-defined ac...
TMap< FString, FActorAttribute > Variations
用户选择了参与者的变化版本。请注意,此时是 由不可修改的属性表示
crp::Actor SerializedData
Definition ActorInfo.h:40
Seting for map generation from opendrive without additional geometry