CARLA
 
载入中...
搜索中...
未找到
RoutePlanner.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"
8#include "RoutePlanner.h"
9
10#include "Util/RandomEngine.h"
13
14#include "Engine/CollisionProfile.h"
15#include "DrawDebugHelpers.h"
16
17// 判断给定的样条组件是否有效,有效条件是组件不为空且样条点数大于1
18static bool IsSplineValid(const USplineComponent *SplineComponent)
19{
20 return (SplineComponent!= nullptr) &&
21 (SplineComponent->GetNumberOfSplinePoints() > 1);
22}
23
24// 获取给定Actor对应的车辆AI控制器,如果Actor即将被销毁则返回nullptr,
25// 先尝试将Actor转换为ACarlaWheeledVehicle类型,若成功再获取其控制器并转换为AWheeledVehicleAIController类型返回
27{
28 auto *Vehicle = (Actor->IsPendingKill()? nullptr : Cast<ACarlaWheeledVehicle>(Actor));
29 return (Vehicle!= nullptr?
30 Cast<AWheeledVehicleAIController>(Vehicle->GetController()) :
31 nullptr);
32}
33
34// 根据给定的随机引擎、路径数组(USplineComponent类型)以及对应的概率数组,
35// 按照概率随机选择一条路径(USplineComponent)并返回。
36// 如果路径数组只有一个元素,则直接返回该元素;
37// 否则根据概率权重选择对应的路径索引,并返回对应路径
38static const USplineComponent *PickARoute(
39 URandomEngine &RandomEngine,
40 const TArray<USplineComponent *> &Routes,
41 const TArray<float> &Probabilities)
42{
43 check(Routes.Num() > 0);
44
45 if (Routes.Num() == 1)
46 {
47 return Routes[0];
48 }
49
50 auto Index = RandomEngine.GetIntWithWeight(Probabilities);
51 check((Index >= 0) && (Index < Routes.Num()));
52 return Routes[Index];
53}
54
55// ARoutePlanner类的构造函数,初始化根组件、触发体组件等相关属性
56ARoutePlanner::ARoutePlanner(const FObjectInitializer &ObjectInitializer)
57 : Super(ObjectInitializer)
58{
59 // 创建默认的场景根组件作为根组件,设置其移动性为静态
60 RootComponent =
61 ObjectInitializer.CreateDefaultSubobject<USceneComponent>(this, TEXT("SceneRootComponent"));
62 RootComponent->SetMobility(EComponentMobility::Static);
63
64 // 创建一个盒体组件作为触发体,用于检测车辆等Actor的重叠事件
65 TriggerVolume = CreateDefaultSubobject<UBoxComponent>(TEXT("TriggerVolume"));
66 TriggerVolume->SetupAttachment(RootComponent);
67 TriggerVolume->SetHiddenInGame(true);
68 TriggerVolume->SetMobility(EComponentMobility::Static);
69 TriggerVolume->SetCollisionProfileName(FName("OverlapAll"));
70 TriggerVolume->SetGenerateOverlapEvents(true);
71
72 // 设置触发体盒体的大小,此处不要随意更改默认值,因为自动驾驶相关功能依赖于此
73 TriggerVolume->SetBoxExtent(FVector{32.0f, 32.0f, 32.0f});
74}
75
76// 在对象即将销毁时调用,清理已有的路径相关数据
78{
79 CleanRoute();
80 Super::BeginDestroy();
81}
82
83#if WITH_EDITOR
84// 在编辑器中属性发生变化后调用,用于处理路径和概率数组相关的逻辑
85// 如果属性发生变化且路径数组和概率数组大小不一致,会重置概率数组并为新的路径创建默认的样条组件等初始化操作
86void ARoutePlanner::PostEditChangeProperty(FPropertyChangedEvent &PropertyChangedEvent)
87{
88 Super::PostEditChangeProperty(PropertyChangedEvent);
89 const auto Size = Routes.Num();
90 if (PropertyChangedEvent.Property && (Size!= Probabilities.Num()))
91 {
92 Probabilities.Reset(Size);
93 for (auto i = 0; i < Size; ++i)
94 {
95 Probabilities.Add(1.0f / static_cast<float>(Size));
96 if (Routes[i] == nullptr)
97 {
98 Routes[i] = NewObject<USplineComponent>(this);
99 Routes[i]->SetupAttachment(RootComponent);
100 Routes[i]->SetHiddenInGame(true);
101 Routes[i]->SetMobility(EComponentMobility::Static);
102 Routes[i]->RegisterComponent();
103 }
104 }
105 }
106}
107#endif // WITH_EDITOR
108
109// 添加一条新的路径,根据给定的概率以及路径上的点数组创建一个新的样条组件表示路径,
110// 并将其添加到路径数组Routes中,同时将对应的概率添加到概率数组Probabilities中
111void ARoutePlanner::AddRoute(float probability, const TArray<FVector> &routePoints)
112{
113 USplineComponent *NewSpline = NewObject<USplineComponent>(this);
114 NewSpline->bHiddenInGame = true;
115
116#if WITH_EDITOR
117 // 在编辑器中设置未选中样条线段的颜色
118 NewSpline->EditorUnselectedSplineSegmentColor = FLinearColor(1.f, 0.15f, 0.15f);
119#endif // WITH_EDITOR
120
121 // 设置样条组件的起始点位置
122 NewSpline->SetLocationAtSplinePoint(0, routePoints[0], ESplineCoordinateSpace::World, true);
123 NewSpline->SetLocationAtSplinePoint(1, routePoints[1], ESplineCoordinateSpace::World, true);
124
125 // 依次添加路径上剩余的点到样条组件中
126 for (int i = 2; i < routePoints.Num(); ++i)
127 {
128 NewSpline->AddSplinePoint(routePoints[i], ESplineCoordinateSpace::World, true);
129 }
130
131 Routes.Add(NewSpline);
132 Probabilities.Add(probability);
133}
134
135// 清空已有的路径数组Routes和概率数组Probabilities,释放相关内存
137{
138 Routes.Empty();
139 Probabilities.Empty();
140}
141
142// 为给定的车辆AI控制器分配一条随机路径,前提是控制器未处于即将销毁状态且其随机引擎有效
143// 通过随机选择一条路径,提取路径上的路点信息,并设置给控制器作为固定路径
145{
146 if (!Controller.IsPendingKill() && (Controller.GetRandomEngine()!= nullptr))
147 {
148 auto *RandomEngine = Controller.GetRandomEngine();
149 auto *Route = PickARoute(*RandomEngine, Routes, Probabilities);
150
151 TArray<FVector> WayPoints;
152 const auto Size = Route->GetNumberOfSplinePoints();
153 if (Size > 1)
154 {
155 WayPoints.Reserve(Size);
156 for (auto i = 1; i < Size; ++i)
157 {
158 WayPoints.Add(Route->GetLocationAtSplinePoint(i, ESplineCoordinateSpace::World));
159 }
160
161 Controller.SetFixedRoute(WayPoints);
162 }
163 else
164 {
165 // 如果路径的样条点数小于等于1,输出错误日志提示
166 UE_LOG(LogCarla, Error, TEXT("ARoutePlanner '%s' has a route with zero way-points."), *GetName());
167 }
168 }
169
170}
171
172// 初始化函数,用于检查路径相关的合法性以及注册触发体的重叠开始事件委托
173// 如果没有分配路径,输出警告日志;
174// 遍历已有的路径,检查路径的样条组件是否有效,无效则输出错误日志并返回;
175// 如果触发体的重叠开始事件委托未绑定当前类的对应函数,则进行绑定
177{
178 if (Routes.Num() < 1)
179 {
180 UE_LOG(LogCarla, Warning, TEXT("ARoutePlanner '%s' has no route assigned."), *GetName());
181 return;
182 }
183
184 for (auto &&Route : Routes)
185 {
186 if (!IsSplineValid(Route))
187 {
188 UE_LOG(LogCarla, Error, TEXT("ARoutePlanner '%s' has a route with zero way-points."), *GetName());
189 return;
190 }
191 }
192
193 // 注册委托,当触发体开始与其他组件重叠时调用OnTriggerBeginOverlap函数
194 if (!TriggerVolume->OnComponentBeginOverlap.IsAlreadyBound(this, &ARoutePlanner::OnTriggerBeginOverlap))
195 {
196 TriggerVolume->OnComponentBeginOverlap.AddDynamic(this, &ARoutePlanner::OnTriggerBeginOverlap);
197 }
198}
199
200// 在游戏开始时调用,调用父类的BeginPlay函数后执行初始化操作
202{
203 Super::BeginPlay();
204 Init();
205}
206
207// 在游戏结束时调用,用于移除触发体重叠开始事件的委托,然后调用父类的EndPlay函数
208void ARoutePlanner::EndPlay(const EEndPlayReason::Type EndPlayReason)
209{
210 // 移除委托,取消绑定OnComponentBeginOverlap事件与OnTriggerBeginOverlap函数的关联
211 if (TriggerVolume->OnComponentBeginOverlap.IsAlreadyBound(this, &ARoutePlanner::OnTriggerBeginOverlap))
212 {
213 TriggerVolume->OnComponentBeginOverlap.RemoveDynamic(this, &ARoutePlanner::OnTriggerBeginOverlap);
214 }
215
216 Super::EndPlay(EndPlayReason);
217}
218
219// 当触发体与其他Actor开始重叠时调用的函数,获取重叠的Actor对应的车辆AI控制器,
220// 如果控制器存在,则为其分配一条随机路径
222 UPrimitiveComponent * /*OverlappedComp*/,
223 AActor *OtherActor,
224 UPrimitiveComponent * /*OtherComp*/,
225 int32 /*OtherBodyIndex*/,
226 bool /*bFromSweep*/,
227 const FHitResult & /*SweepResult*/)
228{
229 auto *Controller = GetVehicleController(OtherActor);
230 if (Controller!= nullptr)
231 {
232 AssignRandomRoute(*Controller);
233 }
234}
235
236// 在编辑器中绘制路径,遍历所有路径的样条点,根据一定规则绘制线段来表示路径,
237// 颜色根据是否是交叉点(bIsIntersection)有所不同,线段粗细也会根据点的位置动态变化
239{
240#if WITH_EDITOR
241 for (int i = 0, lenRoutes = Routes.Num(); i < lenRoutes; ++i)
242 {
243 for (int j = 0, lenNumPoints = Routes[i]->GetNumberOfSplinePoints() - 1; j < lenNumPoints; ++j)
244 {
245 const FVector p0 = Routes[i]->GetLocationAtSplinePoint(j + 0, ESplineCoordinateSpace::World);
246 const FVector p1 = Routes[i]->GetLocationAtSplinePoint(j + 1, ESplineCoordinateSpace::World);
247
248 static const float MinThickness = 3.f;
249 static const float MaxThickness = 15.f;
250
251 const float Dist = (float) j / (float) lenNumPoints;
252 const float OneMinusDist = 1.f - Dist;
253 const float Thickness = OneMinusDist * MaxThickness + MinThickness;
254
255 if (bIsIntersection)
256 {
257 // 从蓝色到黑色渐变绘制线段(如果是交叉点情况)
258 DrawDebugLine(
259 GetWorld(), p0, p1, FColor(0, 0, 255 * OneMinusDist),
260 true, -1.f, 0, Thickness);
261 }
262 else
263 {
264 // 从绿色到黑色渐变绘制线段(非交叉点情况)
265 DrawDebugLine(
266 GetWorld(), p0, p1, FColor(0, 255 * OneMinusDist, 0),
267 true, -1.f, 0, Thickness);
268 }
269 }
270 }
271#endif
272}
UE_LOG(LogCarla, Log, TEXT("UActorDispatcher::Destroying actor: '%s' %x"), *Id, Actor)
TSharedPtr< const FActorInfo > carla::rpc::ActorState UWorld Actor
static AWheeledVehicleAIController * GetVehicleController(AActor *Actor)
static bool IsSplineValid(const USplineComponent *SplineComponent)
static const USplineComponent * PickARoute(URandomEngine &RandomEngine, const TArray< USplineComponent * > &Routes, const TArray< float > &Probabilities)
virtual void BeginPlay() override
virtual void BeginDestroy() override
void AddRoute(float probability, const TArray< FVector > &routePoints)
UBoxComponent * TriggerVolume
ARoutePlanner(const FObjectInitializer &ObjectInitializer)
void AssignRandomRoute(AWheeledVehicleAIController &Controller) const
void OnTriggerBeginOverlap(UPrimitiveComponent *OverlappedComp, AActor *OtherActor, UPrimitiveComponent *OtherComp, int32 OtherBodyIndex, bool bFromSweep, const FHitResult &SweepResult)
TArray< USplineComponent * > Routes
TArray< float > Probabilities
virtual void EndPlay(EEndPlayReason::Type EndPlayReason) override
带可选 AI 的轮式车辆控制器。
void SetFixedRoute(const TArray< FVector > &Locations, bool bOverwriteCurrent=true)
设置启用 Autopilot 时要遵循的固定路线。
int32 GetIntWithWeight(const TArray< float > &Weights)