From 6d71c6513723e115b20d20035e656331e34bfa6f Mon Sep 17 00:00:00 2001
From: nsubiron <nsubiron@cvc.uab.es>
Date: Tue, 2 Apr 2019 15:52:13 +0200
Subject: [PATCH] Fix vehicles missing the route if autopilot enabled too late

---
 CHANGELOG.md                                  |  1 +
 .../Source/Carla/Traffic/RoutePlanner.cpp     | 49 +++++++++++--------
 .../Carla/Source/Carla/Traffic/RoutePlanner.h |  4 ++
 .../Vehicle/WheeledVehicleAIController.cpp    | 24 +++++++++
 4 files changed, 58 insertions(+), 20 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index ef3af8022..21f3520a5 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -44,6 +44,7 @@
   * Fixed geo-reference of Town01 and Town07
   * Fixed floating pillars in Town04
   * Fixed floating building in Town03
+  * Fixed vehicles missing the route if autopilot enabled too late
   * Enhanced stop triggers options
 
 ## CARLA 0.9.4
diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.cpp
index 804092111..22ac56b29 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.cpp
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.cpp
@@ -120,6 +120,33 @@ void ARoutePlanner::CleanRoute()
   Probabilities.Empty();
 }
 
+void ARoutePlanner::AssignRandomRoute(AWheeledVehicleAIController &Controller) const
+{
+  if (!Controller.IsPendingKill() && (Controller.GetRandomEngine() != nullptr))
+  {
+    auto *RandomEngine = Controller.GetRandomEngine();
+    auto *Route = PickARoute(*RandomEngine, Routes, Probabilities);
+
+    TArray<FVector> WayPoints;
+    const auto Size = Route->GetNumberOfSplinePoints();
+    if (Size > 1)
+    {
+      WayPoints.Reserve(Size);
+      for (auto i = 1; i < Size; ++i)
+      {
+        WayPoints.Add(Route->GetLocationAtSplinePoint(i, ESplineCoordinateSpace::World));
+      }
+
+      Controller.SetFixedRoute(WayPoints);
+    }
+    else
+    {
+      UE_LOG(LogCarla, Error, TEXT("ARoutePlanner '%s' has a route with zero way-points."), *GetName());
+    }
+  }
+
+}
+
 void ARoutePlanner::Init()
 {
   if (Routes.Num() < 1)
@@ -170,27 +197,9 @@ void ARoutePlanner::OnTriggerBeginOverlap(
     const FHitResult & /*SweepResult*/)
 {
   auto *Controller = GetVehicleController(OtherActor);
-  auto *RandomEngine = (Controller != nullptr ? Controller->GetRandomEngine() : nullptr);
-  if (RandomEngine != nullptr)
+  if (Controller != nullptr)
   {
-    auto *Route = PickARoute(*RandomEngine, Routes, Probabilities);
-
-    TArray<FVector> WayPoints;
-    const auto Size = Route->GetNumberOfSplinePoints();
-    if (Size > 1)
-    {
-      WayPoints.Reserve(Size);
-      for (auto i = 1; i < Size; ++i)
-      {
-        WayPoints.Add(Route->GetLocationAtSplinePoint(i, ESplineCoordinateSpace::World));
-      }
-
-      Controller->SetFixedRoute(WayPoints);
-    }
-    else
-    {
-      UE_LOG(LogCarla, Error, TEXT("ARoutePlanner '%s' has a route with zero way-points."), *GetName());
-    }
+    AssignRandomRoute(*Controller);
   }
 }
 
diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.h
index 1af540165..a7af98b3a 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.h
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/RoutePlanner.h
@@ -13,6 +13,8 @@
 
 #include "RoutePlanner.generated.h"
 
+class AWheeledVehicleAIController;
+
 /// 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
@@ -41,6 +43,8 @@ public:
 
   void CleanRoute();
 
+  void AssignRandomRoute(AWheeledVehicleAIController &Controller) const;
+
 protected:
 
 #if WITH_EDITOR
diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp
index eb63dfca5..399714392 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp
@@ -8,6 +8,7 @@
 #include "WheeledVehicleAIController.h"
 
 #include "MapGen/RoadMap.h"
+#include "Traffic/RoutePlanner.h"
 #include "Vehicle/CarlaWheeledVehicle.h"
 
 #include "EngineUtils.h"
@@ -167,6 +168,29 @@ void AWheeledVehicleAIController::ConfigureAutopilot(const bool Enable)
       bAutopilotEnabled ?
       ECarlaWheeledVehicleState::FreeDriving :
       ECarlaWheeledVehicleState::AutopilotOff);
+
+  /// @todo Workaround for a race condition between client and server when
+  /// enabling autopilot right after initializing a vehicle.
+  if (bAutopilotEnabled)
+  {
+    for (TActorIterator<ARoutePlanner> It(GetWorld()); It; ++It)
+    {
+      ARoutePlanner *RoutePlanner = *It;
+      // Check if we are inside this route planner.
+      TSet<AActor *> OverlappingActors;
+      RoutePlanner->TriggerVolume->GetOverlappingActors(
+          OverlappingActors,
+          ACarlaWheeledVehicle::StaticClass());
+      for (auto *Actor : OverlappingActors)
+      {
+        if (Actor == Vehicle)
+        {
+          RoutePlanner->AssignRandomRoute(*this);
+          return;
+        }
+      }
+    }
+  }
 }
 
 // =============================================================================