CARLA
OpenDriveActor.cpp
Go to the documentation of this file.
1 // Copyright (c) 2019 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 
11 
12 #include "UObject/ConstructorHelpers.h"
13 #include "DrawDebugHelpers.h"
14 
15 #include "ConstructorHelpers.h"
16 #include "Materials/MaterialExpressionTextureSample.h"
17 
18 #include "DrawDebugHelpers.h"
19 
21 #include <carla/geom/Math.h>
24 #include <carla/rpc/String.h>
26 
27 #include <algorithm>
28 #include <unordered_set>
29 
30 AOpenDriveActor::AOpenDriveActor(const FObjectInitializer &ObjectInitializer)
31  : Super(ObjectInitializer)
32 {
33  PrimaryActorTick.bCanEverTick = false;
34 
35  // Structure to hold one-time initialization
36  static struct FConstructorStatics
37  {
38  // A helper class object we use to find target UTexture2D object in resource
39  // package
40  ConstructorHelpers::FObjectFinderOptional<UTexture2D> TextureObject;
41  FName Category;
42  FText Name;
43  FConstructorStatics()
44  // Use helper class object to find the texture resource path
45  : TextureObject(TEXT("/Carla/Icons/OpenDriveActorIcon")),
46  Category(TEXT("OpenDriveActor")),
47  Name(NSLOCTEXT("SpriteCategory", "OpenDriveActor", "OpenDriveActor"))
48  {}
49  } ConstructorStatics;
50 
51  // We need a scene component to attach Icon sprite
52  USceneComponent *SceneComponent =
53  ObjectInitializer.CreateDefaultSubobject<USceneComponent>(this, TEXT("SceneComp"));
54  RootComponent = SceneComponent;
55  RootComponent->Mobility = EComponentMobility::Static;
56 
57 #if WITH_EDITORONLY_DATA
59  ObjectInitializer.CreateEditorOnlyDefaultSubobject<UBillboardComponent>(this, TEXT("Sprite"));
60  if (SpriteComponent)
61  {
62  // Get the sprite texture from helper class object
63  SpriteComponent->Sprite = ConstructorStatics.TextureObject.Get();
64  // Assign sprite category name
65  SpriteComponent->SpriteInfo.Category = ConstructorStatics.Category;
66  // Assign sprite display name
67  SpriteComponent->SpriteInfo.DisplayName = ConstructorStatics.Name;
68  // Attach sprite to scene component
69  SpriteComponent->SetupAttachment(RootComponent);
70  SpriteComponent->Mobility = EComponentMobility::Static;
71  SpriteComponent->SetEditorScale(1.f);
72  }
73 #endif // WITH_EDITORONLY_DATA
74 }
75 
76 #if WITH_EDITOR
77 void AOpenDriveActor::PostEditChangeProperty(struct FPropertyChangedEvent &Event)
78 {
79  Super::PostEditChangeProperty(Event);
80 
81  const FName PropertyName = (Event.Property != NULL ? Event.Property->GetFName() : NAME_None);
82  if (PropertyName == GET_MEMBER_NAME_CHECKED(AOpenDriveActor, bGenerateRoutes))
83  {
84  if (bGenerateRoutes)
85  {
86  bGenerateRoutes = false;
87 
88  RemoveRoutes(); // Avoid OpenDrive overlapping
89  RemoveSpawners(); // Restart the spawners in case OpenDrive has changed
90  BuildRoutes();
91 
92  if (bAddSpawners)
93  {
94  AddSpawners();
95  }
96  if (bShowDebug)
97  {
98  DebugRoutes();
99  }
100  }
101  }
102  if (PropertyName == GET_MEMBER_NAME_CHECKED(AOpenDriveActor, bRemoveRoutes))
103  {
104  if (bRemoveRoutes)
105  {
106  bRemoveRoutes = false;
107 
109  RemoveSpawners();
110  RemoveRoutes();
111  }
112  }
113  if (PropertyName == GET_MEMBER_NAME_CHECKED(AOpenDriveActor, bShowDebug))
114  {
115  if (bShowDebug)
116  {
117  DebugRoutes();
118  }
119  else
120  {
122  }
123  }
124  if (PropertyName == GET_MEMBER_NAME_CHECKED(AOpenDriveActor, bRemoveCurrentSpawners))
125  {
126  if (bRemoveCurrentSpawners)
127  {
128  bRemoveCurrentSpawners = false;
129 
130  RemoveSpawners();
131  }
132  }
133 }
134 #endif // WITH_EDITOR
135 
137 {
138  BuildRoutes(GetWorld()->GetMapName());
139 }
140 
141 void AOpenDriveActor::BuildRoutes(FString MapName)
142 {
143  using Waypoint = carla::road::element::Waypoint;
144 
145  // As the OpenDrive file has the same name as level, build the path to the
146  // xodr file using the lavel name and the game content directory.
147  const FString XodrContent = UOpenDrive::LoadXODR(MapName);
148 
149  auto map = carla::opendrive::OpenDriveParser::Load(carla::rpc::FromLongFString(XodrContent));
150 
151  if (!map.has_value())
152  {
153  UE_LOG(LogCarla, Error, TEXT("Failed to parse OpenDrive file."));
154  return;
155  }
156 
157  // List with waypoints, each one at the end of each lane of the map
158  const std::vector<Waypoint> LaneWaypoints =
159  map->GenerateWaypointsOnRoadEntries();
160 
161  std::unordered_map<Waypoint, std::vector<Waypoint>> PredecessorMap;
162 
163  for (auto &Wp : LaneWaypoints)
164  {
165  const auto PredecessorsList = map->GetPredecessors(Wp);
166  if (PredecessorsList.empty())
167  {
168  continue;
169  }
170  const auto MinRoadId = *std::min_element(
171  PredecessorsList.begin(),
172  PredecessorsList.end(),
173  [](const auto &WaypointA, const auto &WaypointB) {
174  return WaypointA.road_id < WaypointB.road_id;
175  });
176  PredecessorMap[MinRoadId].emplace_back(Wp);
177  }
178 
179  for (auto &&PredecessorWp : PredecessorMap)
180  {
181  ARoutePlanner *RoutePlanner = nullptr;
182 
183  for (auto &&Wp : PredecessorWp.second)
184  {
185  std::vector<Waypoint> Waypoints;
186  auto CurrentWp = Wp;
187 
188  do
189  {
190  Waypoints.emplace_back(CurrentWp);
191  const auto Successors = map->GetNext(CurrentWp, RoadAccuracy);
192  if (Successors.empty())
193  {
194  break;
195  }
196  if (Successors.front().road_id != Wp.road_id)
197  {
198  break;
199  }
200  CurrentWp = Successors.front();
201  } while (CurrentWp.road_id == Wp.road_id);
202 
203  // connect the last wp of the current road to the first wp of the following road
204  const auto FollowingWp = map->GetSuccessors(CurrentWp);
205  if (!FollowingWp.empty())
206  {
207  Waypoints.emplace_back(FollowingWp.front());
208  }
209 
210  if (Waypoints.size() >= 2)
211  {
212  TArray<FVector> Positions;
213  Positions.Reserve(Waypoints.size());
214  for (int i = 0; i < Waypoints.size(); ++i)
215  {
216  // Add the trigger height because the z position of the points does not
217  // influence on the driver AI and is easy to visualize in the editor
218  Positions.Add(map->ComputeTransform(Waypoints[i]).location +
219  FVector(0.f, 0.f, TriggersHeight));
220  }
221 
222  // If the route planner does not exist, create it
223  if (RoutePlanner == nullptr )
224  {
225  const auto WpTransform = map->ComputeTransform(Wp);
226  RoutePlanner = GetWorld()->SpawnActor<ARoutePlanner>();
227  RoutePlanner->bIsIntersection = map->IsJunction(Wp.road_id);
228  RoutePlanner->SetBoxExtent(FVector(70.f, 70.f, 50.f));
229  RoutePlanner->SetActorRotation(WpTransform.rotation);
230  RoutePlanner->SetActorLocation(WpTransform.location +
231  FVector(0.f, 0.f, TriggersHeight));
232  }
233 
234  if (RoutePlanner != nullptr)
235  {
236  RoutePlanner->AddRoute(1.f, Positions);
237  RoutePlanners.Add(RoutePlanner);
238  }
239  }
240  }
241  }
242 }
243 
245 {
246  const int rp_num = RoutePlanners.Num();
247  for (int i = 0; i < rp_num; i++)
248  {
249  if (RoutePlanners[i] != nullptr)
250  {
251  RoutePlanners[i]->Destroy();
252  }
253  }
254  RoutePlanners.Empty();
255 }
256 
258 {
259  for (int i = 0; i < RoutePlanners.Num(); ++i)
260  {
261  if (RoutePlanners[i] != nullptr)
262  {
263  RoutePlanners[i]->DrawRoutes();
264  }
265  }
266 }
267 
269 {
270 #if WITH_EDITOR
271  FlushPersistentDebugLines(GetWorld());
272 #endif // WITH_EDITOR
273 }
274 
276 {
277  for (int i = 0; i < RoutePlanners.Num(); ++i)
278  {
279  if (RoutePlanners[i] != nullptr)
280  {
281  if (!bOnIntersections && RoutePlanners[i]->bIsIntersection)
282  {
283  continue;
284  }
285  else
286  {
287  FTransform Trans = RoutePlanners[i]->GetActorTransform();
288  AVehicleSpawnPoint *Spawner = GetWorld()->SpawnActor<AVehicleSpawnPoint>();
289  Spawner->SetActorRotation(Trans.GetRotation());
290  Spawner->SetActorLocation(Trans.GetTranslation() + FVector(0.f, 0.f, SpawnersHeight));
291  VehicleSpawners.Add(Spawner);
292  }
293  }
294  }
295 }
296 
298 {
299  const int vs_num = VehicleSpawners.Num();
300  for (int i = 0; i < vs_num; i++)
301  {
302  if (VehicleSpawners[i] != nullptr)
303  {
304  VehicleSpawners[i]->Destroy();
305  }
306  }
307  VehicleSpawners.Empty();
308 }
TArray< AVehicleSpawnPoint * > VehicleSpawners
void DebugRoutes() const
Base class for spawner locations for walkers.
UBillboardComponent * SpriteComponent
A UBillboardComponent to hold Icon sprite.
TArray< ARoutePlanner * > RoutePlanners
Assign a random route to every ACarlaWheeledVehicle entering the trigger volume.
Definition: RoutePlanner.h:23
void RemoveRoutes()
Remove all the existing ARoutePlanner and VehicleSpawners previously generated by this class to avoid...
static boost::optional< road::Map > Load(const std::string &opendrive)
I min_element(I begin, I end, const Pred &pred)
Definition: pugixml.cpp:7347
bool bAddSpawners
If true, spawners will be placed when generating the routes.
void RemoveDebugRoutes() const
void AddRoute(float probability, const TArray< FVector > &routePoints)
float RoadAccuracy
Distance between waypoints where the cars will drive.
bool bIsIntersection
Definition: RoutePlanner.h:80
bool bOnIntersections
If true, spawners will be placed on junctions too.
void SetBoxExtent(const FVector &Extent)
Definition: RoutePlanner.h:35
float TriggersHeight
Trigger elevantion.
float SpawnersHeight
Determine the height where the spawners will be placed, relative to each RoutePlanner.
AOpenDriveActor(const FObjectInitializer &ObjectInitializer)