#2 New RoutePlanner class for handling intersections

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/RoutePlanner.cpp

@@ -0,0 +1,145 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB), and the INTEL Visual Computing Lab.
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#include "Carla.h"
+#include "RoutePlanner.h"
+
+#include "CarlaWheeledVehicle.h"
+#include "Util/RandomEngine.h"
+#include "WheeledVehicleAIController.h"
+
+#include "Engine/CollisionProfile.h"
+
+static bool IsSplineValid(const USplineComponent *SplineComponent)
+{
+  return (SplineComponent != nullptr) &&
+         (SplineComponent->GetNumberOfSplinePoints() > 0);
+}
+
+static AWheeledVehicleAIController *GetVehicleController(AActor *Actor)
+{
+  auto *Vehicle = (Actor->IsPendingKill() ? nullptr : Cast<ACarlaWheeledVehicle>(Actor));
+  return (Vehicle != nullptr ?
+      Cast<AWheeledVehicleAIController>(Vehicle->GetController()) :
+      nullptr);
+}
+
+static const USplineComponent *PickARoute(
+    URandomEngine &RandomEngine,
+    const TArray<USplineComponent *> &Routes,
+    const TArray<float> &Probabilities)
+{
+  check(Routes.Num() > 0);
+
+  if (Routes.Num() == 1) {
+    return Routes[0];
+  }
+
+  auto Index = RandomEngine.GetIntWithWeight(Probabilities);
+  check((Index >= 0) && (Index < Routes.Num()));
+  return Routes[Index];
+}
+
+ARoutePlanner::ARoutePlanner(const FObjectInitializer& ObjectInitializer) :
+  Super(ObjectInitializer)
+{
+  RootComponent =
+      ObjectInitializer.CreateDefaultSubobject<USceneComponent>(this, TEXT("SceneRootComponent"));
+  RootComponent->SetMobility(EComponentMobility::Static);
+
+  TriggerVolume = CreateDefaultSubobject<UBoxComponent>(TEXT("TriggerVolume"));
+  TriggerVolume->SetupAttachment(RootComponent);
+  TriggerVolume->SetHiddenInGame(true);
+  TriggerVolume->SetMobility(EComponentMobility::Static);
+  TriggerVolume->SetCollisionProfileName(FName("OverlapAll"));
+  TriggerVolume->bGenerateOverlapEvents = true;
+}
+
+#if WITH_EDITOR
+void ARoutePlanner::PostEditChangeProperty(FPropertyChangedEvent& PropertyChangedEvent)
+{
+  Super::PostEditChangeProperty(PropertyChangedEvent);
+  const auto Size = Routes.Num();
+  if (PropertyChangedEvent.Property && (Size != Probabilities.Num())) {
+    Probabilities.Reset(Size);
+    for (auto i = 0; i < Size; ++i) {
+      Probabilities.Add(1.0f / static_cast<float>(Size));
+      if (Routes[i] == nullptr) {
+        Routes[i] = NewObject<USplineComponent>(this);
+        Routes[i]->SetupAttachment(RootComponent);
+        Routes[i]->SetHiddenInGame(true);
+        Routes[i]->SetMobility(EComponentMobility::Static);
+        Routes[i]->RegisterComponent();
+      }
+    }
+  }
+}
+#endif // WITH_EDITOR
+
+void ARoutePlanner::BeginPlay()
+{
+  Super::BeginPlay();
+
+  if (Routes.Num() < 1) {
+    UE_LOG(LogCarla, Warning, TEXT("ARoutePlanner has no route assigned."));
+    return;
+  }
+
+  for (auto &&Route : Routes) {
+    if (!IsSplineValid(Route)) {
+      UE_LOG(LogCarla, Error, TEXT("ARoutePlanner has a route with zero way-points."));
+      return;
+    }
+  }
+
+  // Register delegate on begin overlap.
+  if (!TriggerVolume->OnComponentBeginOverlap.IsAlreadyBound(this, &ARoutePlanner::OnTriggerBeginOverlap))
+  {
+    TriggerVolume->OnComponentBeginOverlap.AddDynamic(this, &ARoutePlanner::OnTriggerBeginOverlap);
+  }
+}
+
+void ARoutePlanner::EndPlay(const EEndPlayReason::Type EndPlayReason)
+{
+  // Deregister the delegate.
+  if (TriggerVolume->OnComponentBeginOverlap.IsAlreadyBound(this, &ARoutePlanner::OnTriggerBeginOverlap))
+  {
+    TriggerVolume->OnComponentBeginOverlap.RemoveDynamic(this, &ARoutePlanner::OnTriggerBeginOverlap);
+  }
+
+  Super::EndPlay(EndPlayReason);
+}
+
+void ARoutePlanner::OnTriggerBeginOverlap(
+    UPrimitiveComponent * /*OverlappedComp*/,
+    AActor *OtherActor,
+    UPrimitiveComponent * /*OtherComp*/,
+    int32 /*OtherBodyIndex*/,
+    bool /*bFromSweep*/,
+    const FHitResult & /*SweepResult*/)
+{
+  UE_LOG(LogCarla, Warning, TEXT("Begin overlap!"));
+  auto *Controller = GetVehicleController(OtherActor);
+  auto *RandomEngine = (Controller != nullptr ? Controller->GetRandomEngine() : nullptr);
+  if (RandomEngine != nullptr)
+  {
+    auto *Route = PickARoute(*RandomEngine, Routes, Probabilities);
+
+    TArray<FVector> WayPoints;
+    const auto Size = Route->GetNumberOfSplinePoints();
+    check(Size > 0);
+    WayPoints.Reserve(Size);
+    for (auto i = 0; i < Size; ++i)
+    {
+      WayPoints.Add(Route->GetLocationAtSplinePoint(i, ESplineCoordinateSpace::World));
+    }
+
+    UE_LOG(LogCarla, Log, TEXT("Setting a fixed route of %d points"), WayPoints.Num());
+    Controller->SetFixedRoute(WayPoints);
+  } else {
+    UE_LOG(LogCarla, Error, TEXT("%s has null random engine!"), *OtherActor->GetName());
+  }
+}

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/RoutePlanner.h

@@ -0,0 +1,56 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB), and the INTEL Visual Computing Lab.
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "GameFramework/Actor.h"
+#include "Components/BoxComponent.h"
+#include "Components/SplineComponent.h"
+#include "RoutePlanner.generated.h"
+
+/// Assign a random route to every ACarlaWheeledVehicle entering the trigger
+/// volume. Routes must be added in editor after placing this actor into the
+/// world. Spline tangents are ignored, only locations are taken into account
+/// for making the route.
+UCLASS()
+class CARLA_API ARoutePlanner : public AActor
+{
+  GENERATED_BODY()
+
+public:
+
+  ARoutePlanner(const FObjectInitializer& ObjectInitializer);
+
+protected:
+
+#if WITH_EDITOR
+  virtual void PostEditChangeProperty(FPropertyChangedEvent& PropertyChangedEvent) override;
+#endif // WITH_EDITOR
+
+  virtual void BeginPlay() override;
+
+  virtual void EndPlay(EEndPlayReason::Type EndPlayReason) override;
+
+  UFUNCTION()
+  void OnTriggerBeginOverlap(
+      UPrimitiveComponent* OverlappedComp,
+      AActor *OtherActor,
+      UPrimitiveComponent *OtherComp,
+      int32 OtherBodyIndex,
+      bool bFromSweep,
+      const FHitResult &SweepResult);
+
+public:
+
+  UPROPERTY(EditAnywhere)
+  UBoxComponent *TriggerVolume;
+
+  UPROPERTY(BlueprintReadWrite, Category="Traffic Routes", EditAnywhere)
+  TArray<USplineComponent *> Routes;
+
+  UPROPERTY(BlueprintReadWrite, Category="Traffic Routes", EditAnywhere, EditFixedSize)
+  TArray<float> Probabilities;
+};

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WheeledVehicleAIController.cpp

@@ -59,6 +59,13 @@ static bool IsThereAnObstacleAhead(
       RayTrace(Vehicle, StartLeft, EndLeft);
 }
 
+template <typename T>
+static void ClearQueue(std::queue<T> &Queue)
+{
+  std::queue<T> EmptyQueue;
+  Queue.swap(EmptyQueue);
+}
+
 // =============================================================================
 // -- Constructor and destructor -----------------------------------------------
 // =============================================================================
@@ -118,8 +125,7 @@ void AWheeledVehicleAIController::ConfigureAutopilot(const bool Enable)
   Vehicle->SetReverse(false);
   Vehicle->SetHandbrakeInput(false);
   TrafficLightState = ETrafficLightState::Green;
-  decltype(TargetLocations) EmptyQueue;
-  TargetLocations.swap(EmptyQueue);
+  ClearQueue(TargetLocations);
   Vehicle->SetAIVehicleState(
       bAutopilotEnabled ?
           ECarlaWheeledVehicleState::FreeDriving :
@@ -130,8 +136,13 @@ void AWheeledVehicleAIController::ConfigureAutopilot(const bool Enable)
 // -- Traffic ------------------------------------------------------------------
 // =============================================================================
 
-void AWheeledVehicleAIController::SetFixedRoute(const TArray<FVector> &Locations)
+void AWheeledVehicleAIController::SetFixedRoute(
+    const TArray<FVector> &Locations,
+    const bool bOverwriteCurrent)
 {
+  if (bOverwriteCurrent) {
+    ClearQueue(TargetLocations);
+  }
   for (auto &Location : Locations) {
     TargetLocations.emplace(Location);
   }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/AI/WheeledVehicleAIController.h

@@ -177,7 +177,7 @@ public:
 
   /// Set a fixed route to follow if autopilot is enabled.
   UFUNCTION(Category = "Wheeled Vehicle Controller", BlueprintCallable)
-  void SetFixedRoute(const TArray<FVector> &Locations);
+  void SetFixedRoute(const TArray<FVector> &Locations, bool bOverwriteCurrent=true);
 
   /// @}
   // ===========================================================================