From 7e45899d74f57bc36c812bf261aa0d38f4854bf9 Mon Sep 17 00:00:00 2001
From: Marc Garcia Puig <marcgpuig@gmail.com>
Date: Fri, 25 Jan 2019 21:08:03 +0100
Subject: [PATCH] Fixed vehicle controller raycast and OpenDriveActor issues

---
 .../Carla/Source/Carla/OpenDriveActor.cpp     | 63 ++++++++++++-------
 .../Vehicle/WheeledVehicleAIController.cpp    | 12 ++--
 2 files changed, 46 insertions(+), 29 deletions(-)

diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp
index 247706ca6..b36b9f796 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp
@@ -169,11 +169,16 @@ void AOpenDriveActor::BuildRoutes()
   const std::vector<Waypoint> MapLaneBeginWaypoint =
       WaypointGen::GenerateLaneEnd(*map_ptr);
 
+  // Since we are going to iterate all the successors of all the lanes, we need
+  // a vector to store the already visited lanes. Lanes can be successors of
+  // multiple other lanes
+  std::vector<std::pair<IdType, int>> AlreadyVisited;
+
   for (auto &&EndLaneWaypoint : MapLaneBeginWaypoint)
   {
     std::vector<Waypoint> Successors = WaypointGen::GetSuccessors(EndLaneWaypoint);
 
-    // generate the RoutePlanner
+    // Generate the RoutePlanner
     ARoutePlanner *RoutePlanner = GetWorld()->SpawnActor<ARoutePlanner>();
     RoutePlanner->bIsIntersection = std::any_of(Successors.begin(), Successors.end(), [](auto w) {
       return w.IsIntersection();
@@ -183,36 +188,50 @@ void AOpenDriveActor::BuildRoutes()
     RoutePlanner->SetActorLocation(EndLaneWaypoint.ComputeTransform().location +
         FVector(0.f, 0.f, TriggersHeight));
 
-    // fill the RoutePlanner with all the needed roads
+    // Fill the RoutePlanner with all the needed roads
     for (auto &&Successor : Successors)
     {
       const IdType RoadId = Successor.GetRoadId();
-      const float MaxDist = map.GetRoad(RoadId)->GetLength();
+      const int LaneId = Successor.GetLaneId();
 
-      std::vector<Waypoint> Waypoints;
+      // Create an identifier of the current lane
+      const auto Identifier = std::make_pair(RoadId, LaneId);
 
-      Waypoints.emplace_back(Successor);
-
-      for (float Dist = RoadAccuracy; Dist < MaxDist; Dist += RoadAccuracy)
+      // If we haven't visited the lane
+      if (!std::any_of(AlreadyVisited.begin(), AlreadyVisited.end(), [&Identifier](auto i) {
+        return (i.first == Identifier.first && i.second == Identifier.second);
+      }))
       {
-        const auto NewWaypoint = WaypointGen::GetNext(Successor, Dist);
+        // Add the identifier as visited
+        AlreadyVisited.emplace_back(Identifier);
 
-        check(Dist < MaxDist);
-        check(NewWaypoint.size() == 1);
+        const float MaxDist = map.GetRoad(RoadId)->GetLength();
 
-        Waypoints.emplace_back(NewWaypoint[0]);
+        std::vector<Waypoint> Waypoints;
+
+        Waypoints.emplace_back(Successor);
+
+        for (float Dist = RoadAccuracy; Dist < MaxDist; Dist += RoadAccuracy)
+        {
+          const auto NewWaypoint = WaypointGen::GetNext(Successor, Dist);
+
+          check(Dist < MaxDist);
+          check(NewWaypoint.size() == 1);
+
+          Waypoints.emplace_back(NewWaypoint[0]);
+        }
+
+        // Merge with the first waypoint of the next lane if needed
+        Waypoints.emplace_back(WaypointGen::GetNext(
+            Successor, CarlaMath::clamp(MaxDist - 0.1f, 0.f, MaxDist))[0]);
+
+        check(Waypoints.size() >= 2);
+
+        TArray<FVector> Positions = WaypointVector2FVectorArray(Waypoints, TriggersHeight);
+
+        RoutePlanner->AddRoute(1.f, Positions);
+        RoutePlanners.Add(RoutePlanner);
       }
-
-      // merge with the first waypoint of the next lane if needed
-      Waypoints.emplace_back(WaypointGen::GetNext(
-          Successor, CarlaMath::clamp(MaxDist - 0.1f, 0.f, MaxDist))[0]);
-
-      check(Waypoints.size() >= 2);
-
-      TArray<FVector> Positions = WaypointVector2FVectorArray(Waypoints, TriggersHeight);
-
-      RoutePlanner->AddRoute(1.f, Positions);
-      RoutePlanners.Add(RoutePlanner);
     }
   }
 }
diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp
index 2a6fb8921..c753f5313 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/WheeledVehicleAIController.cpp
@@ -42,16 +42,18 @@ static bool IsThereAnObstacleAhead(
   const auto ForwardVector = Vehicle.GetVehicleOrientation();
   const auto VehicleBounds = Vehicle.GetVehicleBoundingBoxExtent();
 
+  FVector NormDirection = Direction.GetSafeNormal();
+
   const float Distance = std::max(50.0f, Speed * Speed); // why?
 
   const FVector StartCenter = Vehicle.GetActorLocation() + (ForwardVector * (250.0f + VehicleBounds.X / 2.0f)) + FVector(0.0f, 0.0f, 50.0f);
-  const FVector EndCenter = StartCenter + Direction * (Distance + VehicleBounds.X / 2.0f);
+  const FVector EndCenter = StartCenter + NormDirection * (Distance + VehicleBounds.X / 2.0f);
 
   const FVector StartRight = StartCenter + (FVector(ForwardVector.Y, -ForwardVector.X, ForwardVector.Z) * 100.0f);
-  const FVector EndRight = StartRight + Direction * (Distance + VehicleBounds.X / 2.0f);
+  const FVector EndRight = StartRight + NormDirection * (Distance + VehicleBounds.X / 2.0f);
 
   const FVector StartLeft = StartCenter + (FVector(-ForwardVector.Y, ForwardVector.X, ForwardVector.Z) * 100.0f);
-  const FVector EndLeft = StartLeft + Direction * (Distance + VehicleBounds.X / 2.0f);
+  const FVector EndLeft = StartLeft + NormDirection * (Distance + VehicleBounds.X / 2.0f);
 
   return
       RayTrace(Vehicle, StartCenter, EndCenter) ||
@@ -185,10 +187,6 @@ void AWheeledVehicleAIController::TickAutopilotController()
 
   check(Vehicle != nullptr);
 
-  if (RoadMap == nullptr) {
-    return;
-  }
-
   FVector Direction;
 
   float Steering;