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