Merge master into local_planner_debug_flag

CHANGELOG.md

@@ -16,11 +16,13 @@
       - Walkers animation is simulated in playback (through speed of walker), so they walk properly.
   * Fixed Lidar effectiveness bug in manual_control.py
   * Added C++ client example using LibCarla
+  * Updated OpenDriveActor to use the new Waypoint API
   * Fixed wrong units in VehiclePhysicsControl's center of mass
   * Several optimizations to the RPC server, now supports a bigger load of async messages
 
 ## CARLA 0.9.5
 
+  * Added `client_bounding_boxes.py` to show bounding boxes client-side
   * New Town07, rural environment with narrow roads
   * Reworked OpenDRIVE parser and waypoints API
     - Fixed several situations in which the XODR was incorrectly parsed

LibCarla/source/carla/road/Map.cpp

@@ -49,6 +49,14 @@ namespace road {
     }
   }
 
+  static double GetDistanceAtEndOfLane(const Lane &lane) {
+    if (lane.GetId() > 0) {
+      return lane.GetDistance() + 10.0 * EPSILON;
+    } else {
+      return lane.GetDistance() + lane.GetLength() - 10.0 * EPSILON;
+    }
+  }
+
   /// Return a waypoint for each drivable lane on @a lane_section.
   template <typename FuncT>
   static void ForEachDrivableLaneImpl(
@@ -377,6 +385,24 @@ namespace road {
     return result;
   }
 
+  std::vector<Waypoint> Map::GetPredecessors(const Waypoint waypoint) const {
+    const auto &prev_lanes = GetLane(waypoint).GetPreviousLanes();
+    std::vector<Waypoint> result;
+    result.reserve(prev_lanes.size());
+    for (auto *next_lane : prev_lanes) {
+      RELEASE_ASSERT(next_lane != nullptr);
+      const auto lane_id = next_lane->GetId();
+      RELEASE_ASSERT(lane_id != 0);
+      const auto *section = next_lane->GetLaneSection();
+      RELEASE_ASSERT(section != nullptr);
+      const auto *road = next_lane->GetRoad();
+      RELEASE_ASSERT(road != nullptr);
+      const auto distance = GetDistanceAtEndOfLane(*next_lane);
+      result.emplace_back(Waypoint{road->GetId(), section->GetId(), lane_id, distance});
+    }
+    return result;
+  }
+
   std::vector<Waypoint> Map::GetNext(
       const Waypoint waypoint,
       const double distance) const {
@@ -445,6 +471,33 @@ namespace road {
     return result;
   }
 
+  std::vector<Waypoint> Map::GenerateWaypointsOnRoadEntries() const {
+    std::vector<Waypoint> result;
+    for (const auto &pair : _data.GetRoads()) {
+      const auto &road = pair.second;
+      // right lanes start at s 0
+      for (const auto &lane_section : road.GetLaneSectionsAt(0.0)) {
+        for (const auto &lane : lane_section.GetLanes()) {
+          // add only the right (negative) lanes
+          if (lane.first < 0 && lane.second.GetType() == Lane::LaneType::Driving) {
+            result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
+          }
+        }
+      }
+      // left lanes start at s max
+      const auto road_len = road.GetLength();
+      for (const auto &lane_section : road.GetLaneSectionsAt(road_len)) {
+        for (const auto &lane : lane_section.GetLanes()) {
+          // add only the left (positive) lanes
+          if (lane.first > 0 && lane.second.GetType() == Lane::LaneType::Driving) {
+            result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
+          }
+        }
+      }
+    }
+    return result;
+  }
+
   std::vector<std::pair<Waypoint, Waypoint>> Map::GenerateTopology() const {
     std::vector<std::pair<Waypoint, Waypoint>> result;
     for (const auto &pair : _data.GetRoads()) {

LibCarla/source/carla/road/Map.h

@@ -83,6 +83,7 @@ namespace road {
     /// successor lane; i.e., the list of each waypoint in the next road segment
     /// that a vehicle could drive from @a waypoint.
     std::vector<Waypoint> GetSuccessors(Waypoint waypoint) const;
+    std::vector<Waypoint> GetPredecessors(Waypoint waypoint) const;
 
     /// Return the list of waypoints at @a distance such that a vehicle at @a
     /// waypoint could drive to.
@@ -97,6 +98,9 @@ namespace road {
     /// Generate all the waypoints in @a map separated by @a approx_distance.
     std::vector<Waypoint> GenerateWaypoints(double approx_distance) const;
 
+    /// Generate waypoints on each @a lane at the start of each @a road
+    std::vector<Waypoint> GenerateWaypointsOnRoadEntries() const;
+
     /// Generate the minimum set of waypoints that define the topology of @a
     /// map. The waypoints are placed at the entrance of each lane.
     std::vector<std::pair<Waypoint, Waypoint>> GenerateTopology() const;

LibCarla/source/carla/road/Road.h

@@ -63,9 +63,10 @@ namespace road {
 
     Lane *GetPrevLane(const double s, const LaneId lane_id);
 
-    // get the start and end section with a lan id
+    /// Get the start section (from road coordinates s) given a @a lane id
     LaneSection *GetStartSection(LaneId id);
 
+    /// Get the end section (from road coordinates s) given a @a lane id
     LaneSection *GetEndSection(LaneId id);
 
     std::vector<Road *> GetNexts() const;

PythonAPI/.pylintrc

@@ -1,6 +1,6 @@
 [MESSAGES CONTROL]
 max-line-length=120
 [MASTER]
-disable=I0011,I0013,E1121
+disable=I0011,I0013,E1121,E1126
 [TYPECHECK]
 ignored-modules=carla,carla.command,libcarla,pygame,numpy,configparser,ConfigParser

PythonAPI/examples/client_bounding_boxes.py

@@ -0,0 +1,399 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2019 Aptiv
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see <https://opensource.org/licenses/MIT>.
+
+"""
+An example of client-side bounding boxes with basic car controls.
+
+Controls:
+
+    W            : throttle
+    S            : brake
+    AD           : steer
+    Space        : hand-brake
+
+    ESC          : quit
+"""
+
+# ==============================================================================
+# -- find carla module ---------------------------------------------------------
+# ==============================================================================
+
+
+import glob
+import os
+import sys
+
+try:
+    sys.path.append(glob.glob('**/carla-*%d.%d-%s.egg' % (
+        sys.version_info.major,
+        sys.version_info.minor,
+        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
+except IndexError:
+    pass
+
+
+# ==============================================================================
+# -- imports -------------------------------------------------------------------
+# ==============================================================================
+
+import carla
+
+import weakref
+import random
+
+try:
+    import pygame
+    from pygame.locals import K_ESCAPE
+    from pygame.locals import K_SPACE
+    from pygame.locals import K_a
+    from pygame.locals import K_d
+    from pygame.locals import K_s
+    from pygame.locals import K_w
+except ImportError:
+    raise RuntimeError('cannot import pygame, make sure pygame package is installed')
+
+try:
+    import numpy as np
+except ImportError:
+    raise RuntimeError('cannot import numpy, make sure numpy package is installed')
+
+VIEW_WIDTH = 1920//2
+VIEW_HEIGHT = 1080//2
+VIEW_FOV = 90
+
+BB_COLOR = (248, 64, 24)
+
+# ==============================================================================
+# -- ClientSideBoundingBoxes ---------------------------------------------------
+# ==============================================================================
+
+
+class ClientSideBoundingBoxes(object):
+    """
+    This is a module responsible for creating 3D bounding boxes and drawing them
+    client-side on pygame surface.
+    """
+
+    @staticmethod
+    def get_bounding_boxes(vehicles, camera):
+        """
+        Creates 3D bounding boxes based on carla vehicle list and camera.
+        """
+
+        bounding_boxes = [ClientSideBoundingBoxes.get_bounding_box(vehicle, camera) for vehicle in vehicles]
+        # filter objects behind camera
+        bounding_boxes = [bb for bb in bounding_boxes if all(bb[:, 2] > 0)]
+        return bounding_boxes
+
+    @staticmethod
+    def draw_bounding_boxes(display, bounding_boxes):
+        """
+        Draws bounding boxes on pygame display.
+        """
+
+        bb_surface = pygame.Surface((VIEW_WIDTH, VIEW_HEIGHT))
+        bb_surface.set_colorkey((0, 0, 0))
+        for bbox in bounding_boxes:
+            points = [(int(bbox[i, 0]), int(bbox[i, 1])) for i in range(8)]
+            # draw lines
+            # base
+            pygame.draw.line(bb_surface, BB_COLOR, points[0], points[1])
+            pygame.draw.line(bb_surface, BB_COLOR, points[0], points[1])
+            pygame.draw.line(bb_surface, BB_COLOR, points[1], points[2])
+            pygame.draw.line(bb_surface, BB_COLOR, points[2], points[3])
+            pygame.draw.line(bb_surface, BB_COLOR, points[3], points[0])
+            # top
+            pygame.draw.line(bb_surface, BB_COLOR, points[4], points[5])
+            pygame.draw.line(bb_surface, BB_COLOR, points[5], points[6])
+            pygame.draw.line(bb_surface, BB_COLOR, points[6], points[7])
+            pygame.draw.line(bb_surface, BB_COLOR, points[7], points[4])
+            # base-top
+            pygame.draw.line(bb_surface, BB_COLOR, points[0], points[4])
+            pygame.draw.line(bb_surface, BB_COLOR, points[1], points[5])
+            pygame.draw.line(bb_surface, BB_COLOR, points[2], points[6])
+            pygame.draw.line(bb_surface, BB_COLOR, points[3], points[7])
+        display.blit(bb_surface, (0, 0))
+
+    @staticmethod
+    def get_bounding_box(vehicle, camera):
+        """
+        Returns 3D bounding box for a vehicle based on camera view.
+        """
+
+        bb_cords = ClientSideBoundingBoxes._create_bb_points(vehicle)
+        cords_x_y_z = ClientSideBoundingBoxes._vehicle_to_sensor(bb_cords, vehicle, camera)[:3, :]
+        cords_y_minus_z_x = np.concatenate([cords_x_y_z[1, :], -cords_x_y_z[2, :], cords_x_y_z[0, :]])
+        bbox = np.transpose(np.dot(camera.calibration, cords_y_minus_z_x))
+        camera_bbox = np.concatenate([bbox[:, 0] / bbox[:, 2], bbox[:, 1] / bbox[:, 2], bbox[:, 2]], axis=1)
+        return camera_bbox
+
+    @staticmethod
+    def _create_bb_points(vehicle):
+        """
+        Returns 3D bounding box for a vehicle.
+        """
+
+        cords = np.zeros((8, 4))
+        extent = vehicle.bounding_box.extent
+        cords[0, :] = np.array([extent.x, extent.y, -extent.z, 1])
+        cords[1, :] = np.array([-extent.x, extent.y, -extent.z, 1])
+        cords[2, :] = np.array([-extent.x, -extent.y, -extent.z, 1])
+        cords[3, :] = np.array([extent.x, -extent.y, -extent.z, 1])
+        cords[4, :] = np.array([extent.x, extent.y, extent.z, 1])
+        cords[5, :] = np.array([-extent.x, extent.y, extent.z, 1])
+        cords[6, :] = np.array([-extent.x, -extent.y, extent.z, 1])
+        cords[7, :] = np.array([extent.x, -extent.y, extent.z, 1])
+        return cords
+
+    @staticmethod
+    def _vehicle_to_sensor(cords, vehicle, sensor):
+        """
+        Transforms coordinates of a vehicle bounding box to sensor.
+        """
+
+        world_cord = ClientSideBoundingBoxes._vehicle_to_world(cords, vehicle)
+        sensor_cord = ClientSideBoundingBoxes._world_to_sensor(world_cord, sensor)
+        return sensor_cord
+
+    @staticmethod
+    def _vehicle_to_world(cords, vehicle):
+        """
+        Transforms coordinates of a vehicle bounding box to world.
+        """
+
+        bb_transform = carla.Transform(vehicle.bounding_box.location)
+        bb_vehicle_matrix = ClientSideBoundingBoxes.get_matrix(bb_transform)
+        vehicle_world_matrix = ClientSideBoundingBoxes.get_matrix(vehicle.get_transform())
+        bb_world_matrix = np.dot(vehicle_world_matrix, bb_vehicle_matrix)
+        world_cords = np.dot(bb_world_matrix, np.transpose(cords))
+        return world_cords
+
+    @staticmethod
+    def _world_to_sensor(cords, sensor):
+        """
+        Transforms world coordinates to sensor.
+        """
+
+        sensor_world_matrix = ClientSideBoundingBoxes.get_matrix(sensor.get_transform())
+        world_sensor_matrix = np.linalg.inv(sensor_world_matrix)
+        sensor_cords = np.dot(world_sensor_matrix, cords)
+        return sensor_cords
+
+    @staticmethod
+    def get_matrix(transform):
+        """
+        Creates matrix from carla transform.
+        """
+
+        rotation = transform.rotation
+        location = transform.location
+        c_y = np.cos(np.radians(rotation.yaw))
+        s_y = np.sin(np.radians(rotation.yaw))
+        c_r = np.cos(np.radians(rotation.roll))
+        s_r = np.sin(np.radians(rotation.roll))
+        c_p = np.cos(np.radians(rotation.pitch))
+        s_p = np.sin(np.radians(rotation.pitch))
+        matrix = np.matrix(np.identity(4))
+        matrix[0, 3] = location.x
+        matrix[1, 3] = location.y
+        matrix[2, 3] = location.z
+        matrix[0, 0] = c_p * c_y
+        matrix[0, 1] = c_y * s_p * s_r - s_y * c_r
+        matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r
+        matrix[1, 0] = s_y * c_p
+        matrix[1, 1] = s_y * s_p * s_r + c_y * c_r
+        matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r
+        matrix[2, 0] = s_p
+        matrix[2, 1] = -c_p * s_r
+        matrix[2, 2] = c_p * c_r
+        return matrix
+
+
+# ==============================================================================
+# -- BasicSynchronousClient ----------------------------------------------------
+# ==============================================================================
+
+
+class BasicSynchronousClient(object):
+    """
+    Basic implementation of a synchronous client.
+    """
+
+    def __init__(self):
+        self.client = None
+        self.world = None
+        self.camera = None
+        self.car = None
+
+        self.display = None
+        self.image = None
+        self.capture = True
+
+    def camera_blueprint(self):
+        """
+        Returns camera blueprint.
+        """
+
+        camera_bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
+        camera_bp.set_attribute('image_size_x', str(VIEW_WIDTH))
+        camera_bp.set_attribute('image_size_y', str(VIEW_HEIGHT))
+        camera_bp.set_attribute('fov', str(VIEW_FOV))
+        return camera_bp
+
+    def set_synchronous_mode(self, synchronous_mode):
+        """
+        Sets synchronous mode.
+        """
+
+        settings = self.world.get_settings()
+        settings.synchronous_mode = synchronous_mode
+        self.world.apply_settings(settings)
+
+    def setup_car(self):
+        """
+        Spawns actor-vehicle to be controled.
+        """
+
+        car_bp = self.world.get_blueprint_library().filter('vehicle.*')[0]
+        location = random.choice(self.world.get_map().get_spawn_points())
+        self.car = self.world.spawn_actor(car_bp, location)
+
+    def setup_camera(self):
+        """
+        Spawns actor-camera to be used to render view.
+        Sets calibration for client-side boxes rendering.
+        """
+
+        camera_transform = carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15))
+        self.camera = self.world.spawn_actor(self.camera_blueprint(), camera_transform, attach_to=self.car)
+        weak_self = weakref.ref(self)
+        self.camera.listen(lambda image: weak_self().set_image(weak_self, image))
+
+        calibration = np.identity(3)
+        calibration[0, 2] = VIEW_WIDTH / 2.0
+        calibration[1, 2] = VIEW_HEIGHT / 2.0
+        calibration[0, 0] = calibration[1, 1] = VIEW_WIDTH / (2.0 * np.tan(VIEW_FOV * np.pi / 360.0))
+        self.camera.calibration = calibration
+
+    def control(self, car):
+        """
+        Applies control to main car based on pygame pressed keys.
+        Will return True If ESCAPE is hit, otherwise False to end main loop.
+        """
+
+        keys = pygame.key.get_pressed()
+        if keys[K_ESCAPE]:
+            return True
+
+        control = car.get_control()
+        control.throttle = 0
+        if keys[K_w]:
+            control.throttle = 1
+            control.reverse = False
+        elif keys[K_s]:
+            control.throttle = 1
+            control.reverse = True
+        if keys[K_a]:
+            control.steer = max(-1., min(control.steer - 0.05, 0))
+        elif keys[K_d]:
+            control.steer = min(1., max(control.steer + 0.05, 0))
+        else:
+            control.steer = 0
+        control.hand_brake = keys[K_SPACE]
+
+        car.apply_control(control)
+        return False
+
+    @staticmethod
+    def set_image(weak_self, img):
+        """
+        Sets image coming from camera sensor.
+        The self.capture flag is a mean of synchronization - once the flag is
+        set, next coming image will be stored.
+        """
+
+        self = weak_self()
+        if self.capture:
+            self.image = img
+            self.capture = False
+
+    def render(self, display):
+        """
+        Transforms image from camera sensor and blits it to main pygame display.
+        """
+
+        if self.image is not None:
+            array = np.frombuffer(self.image.raw_data, dtype=np.dtype("uint8"))
+            array = np.reshape(array, (self.image.height, self.image.width, 4))
+            array = array[:, :, :3]
+            array = array[:, :, ::-1]
+            surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
+            display.blit(surface, (0, 0))
+
+    def game_loop(self):
+        """
+        Main program loop.
+        """
+
+        try:
+            pygame.init()
+
+            self.client = carla.Client('127.0.0.1', 2000)
+            self.client.set_timeout(2.0)
+            self.world = self.client.get_world()
+
+            self.setup_car()
+            self.setup_camera()
+
+            self.display = pygame.display.set_mode((VIEW_WIDTH, VIEW_HEIGHT), pygame.HWSURFACE | pygame.DOUBLEBUF)
+            pygame_clock = pygame.time.Clock()
+
+            self.set_synchronous_mode(True)
+            vehicles = self.world.get_actors().filter('vehicle.*')
+
+            while True:
+                self.world.tick()
+
+                self.capture = True
+                pygame_clock.tick_busy_loop(20)
+
+                self.render(self.display)
+                bounding_boxes = ClientSideBoundingBoxes.get_bounding_boxes(vehicles, self.camera)
+                ClientSideBoundingBoxes.draw_bounding_boxes(self.display, bounding_boxes)
+
+                pygame.display.flip()
+
+                pygame.event.pump()
+                if self.control(self.car):
+                    return
+
+        finally:
+            self.set_synchronous_mode(False)
+            self.camera.destroy()
+            self.car.destroy()
+            pygame.quit()
+
+
+# ==============================================================================
+# -- main() --------------------------------------------------------------------
+# ==============================================================================
+
+
+def main():
+    """
+    Initializes the client-side bounding box demo.
+    """
+
+    try:
+        client = BasicSynchronousClient()
+        client.game_loop()
+    finally:
+        print('EXIT')
+
+
+if __name__ == '__main__':
+    main()

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.cpp

@@ -16,57 +16,13 @@
 #include <carla/rpc/String.h>
 #include <compiler/enable-ue4-macros.h>
 
+#include <algorithm>
 #include <unordered_set>
 
-/*static TArray<FVector> WaypointVector2FVectorArray(
-    const std::vector<carla::road::element::Waypoint> &Waypoints,
-    const float TriggersHeight)
-{
-  TArray<FVector> Positions;
-  Positions.Reserve(Waypoints.size());
-  for (int i = 0; i < Waypoints.size(); ++i)
-  {
-    // Add the trigger height because the z position of the points does not
-    // influence on the driver AI and is easy to visualize in the editor
-    Positions.Add(Waypoints[i].ComputeTransform().location +
-        FVector(0.f, 0.f, TriggersHeight));
-  }
-  return Positions;
-}*/
-
 AOpenDriveActor::AOpenDriveActor(const FObjectInitializer &ObjectInitializer)
   : Super(ObjectInitializer)
 {
   PrimaryActorTick.bCanEverTick = false;
-  #if WITH_EDITORONLY_DATA
-  static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficLightBP(TEXT(
-      "Blueprint'/Game/Carla/Static/TrafficSigns/Streetlights_01/BP_TrafficLightPole.BP_TrafficLightPole'"));
-  TrafficLightBlueprintClass = (UClass *) TrafficLightBP.Object->GeneratedClass;
-
-  static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficGroupBP(TEXT(
-      "Blueprint'/Game/Carla/Static/TrafficSigns/Streetlights_01/BP_TrafficLightPoleGroup.BP_TrafficLightPoleGroup'"));
-  TrafficGroupBlueprintClass = (UClass *) TrafficGroupBP.Object->GeneratedClass;
-
-  static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficSign30BP(TEXT(
-      "Blueprint'/Game/Carla/Static/TrafficSigns/PostSigns/Round/SpeedLimiters/BP_SpeedLimit30.BP_SpeedLimit30'"));
-  TrafficSign30BlueprintClass = (UClass *) TrafficSign30BP.Object->GeneratedClass;
-
-  /*static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficSign40BP(TEXT(
-      "Blueprint'/Game/Carla/Static/TrafficSigns/PostSigns/Round/SpeedLimiters/BP_SpeedLimit40.BP_SpeedLimit40'"));
-  TrafficSign40BlueprintClass = (UClass *) TrafficSign40BP.Object->GeneratedClass;*/
-
-  static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficSign60BP(TEXT(
-      "Blueprint'/Game/Carla/Static/TrafficSigns/PostSigns/Round/SpeedLimiters/BP_SpeedLimit60.BP_SpeedLimit60'"));
-  TrafficSign60BlueprintClass = (UClass *) TrafficSign60BP.Object->GeneratedClass;
-
-  static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficSign90BP(TEXT(
-      "Blueprint'/Game/Carla/Static/TrafficSigns/PostSigns/Round/SpeedLimiters/BP_SpeedLimit90.BP_SpeedLimit90'"));
-  TrafficSign90BlueprintClass = (UClass *) TrafficSign90BP.Object->GeneratedClass;
-
-  static ConstructorHelpers::FObjectFinder<UBlueprint> TrafficSign100BP(TEXT(
-      "Blueprint'/Game/Carla/Static/TrafficSigns/PostSigns/Round/SpeedLimiters/BP_SpeedLimit100.BP_SpeedLimit100'"));
-  TrafficSign100BlueprintClass = (UClass *) TrafficSign100BP.Object->GeneratedClass;
-  #endif // WITH_EDITORONLY_DATA
 
   // Structure to hold one-time initialization
   static struct FConstructorStatics
@@ -191,219 +147,90 @@ void AOpenDriveActor::BuildRoutes(FString MapName)
   }
 
   // List with waypoints, each one at the end of each lane of the map
-  const std::vector<std::pair<Waypoint, Waypoint>> Topology =
+  const std::vector<Waypoint> LaneWaypoints =
-      map->GenerateTopology();
+      map->GenerateWaypointsOnRoadEntries();
 
-  // Since we are going to iterate all the successors of all the lanes, we need
+  std::unordered_map<Waypoint, std::vector<Waypoint>> PredecessorMap;
-  // a vector to store the already visited lanes. Lanes can be successors of
-  // multiple other lanes
-  std::vector<Waypoint> AlreadyVisited;
 
-  std::unordered_set<Waypoint> WpLaneStartList;
+  for (auto &Wp : LaneWaypoints)
-  for (auto &&WpPair : Topology)
   {
-    WpLaneStartList.emplace(WpPair.first);
+    const auto PredecessorsList = map->GetPredecessors(Wp);
-    WpLaneStartList.emplace(WpPair.second);
+    if (PredecessorsList.empty())
+    {
+      continue;
+    }
+    const auto MinRoadId = *std::min_element(
+        PredecessorsList.begin(),
+        PredecessorsList.end(),
+        [](const auto &WaypointA, const auto &WaypointB) {
+          return WaypointA.road_id < WaypointB.road_id;
+        });
+    PredecessorMap[MinRoadId].emplace_back(Wp);
   }
 
-  for (auto &&Wp : WpLaneStartList)
+  for (auto &&PredecessorWp : PredecessorMap)
   {
-
-    std::vector<Waypoint> Successors = map->GetSuccessors(Wp);
-
-    // // The RoutePlanner will be created only if some route must be added to it
-    // // so no one will be created unnecessarily
     ARoutePlanner *RoutePlanner = nullptr;
 
-    // // Fill the RoutePlanner with all the needed roads
+    for (auto &&Wp : PredecessorWp.second)
-    for (auto &&Successor : Successors)
+    {
-     {
+      std::vector<Waypoint> Waypoints;
-        const auto RoadId = Successor.road_id;
+      auto CurrentWp = Wp;
-        const auto LaneId = Successor.lane_id;
 
-
+      do
-    //   // Create an identifier of the current lane
+      {
-        //const auto Identifier = std::make_pair(RoadId, LaneId);
+        Waypoints.emplace_back(CurrentWp);
-
+        const auto Successors = map->GetNext(CurrentWp, RoadAccuracy);
-    //   // If Identifier does not exist in AlreadyVisited we haven't visited the
+        if (Successors.empty())
-    //   // lane
-        if (!std::any_of(AlreadyVisited.begin(), AlreadyVisited.end(), [&](auto i) {
-          return (i.road_id == Successor.road_id && i.lane_id == Successor.lane_id  && i.section_id == Successor.section_id);
-        }))
         {
-          // Add the identifier as visited
+          break;
-          AlreadyVisited.emplace_back(Successor);
+        }
+        if (Successors.front().road_id != Wp.road_id)
+        {
+          break;
+        }
+        CurrentWp = Successors.front();
+      } while (CurrentWp.road_id == Wp.road_id);
 
-          const double MaxDist = map->GetLane(Successor).GetLength();
+      // connect the last wp of the current toad to the first wp of the following road
-          //const double MaxDist = 0; // workarround while changing the WaypointAPI
+      const auto FollowingWp = map->GetSuccessors(CurrentWp);
+      if (!FollowingWp.empty())
+      {
+        Waypoints.emplace_back(FollowingWp.front());
+      }
 
-          std::vector<Waypoint> Waypoints;
+      if (Waypoints.size() >= 2)
-          // if(RoutePlanner==nullptr) {
+      {
-          //   AlreadyVisited.emplace_back(Wp);
+        TArray<FVector> Positions;
-          //   Waypoints.emplace_back(Wp);
+        Positions.Reserve(Waypoints.size());
-          // }
+        for (int i = 0; i < Waypoints.size(); ++i)
-          Waypoints.emplace_back(Successor);
+        {
+          // Add the trigger height because the z position of the points does not
+          // influence on the driver AI and is easy to visualize in the editor
+          Positions.Add(map->ComputeTransform(Waypoints[i]).location +
+              FVector(0.f, 0.f, TriggersHeight));
+        }
 
-          double Dist = RoadAccuracy;
+        // If the route planner does not exist, create it
-          while (Dist < MaxDist) {
+        if (RoutePlanner == nullptr )
-            auto NewWaypointList = map->GetNext(Successor, Dist);
+        {
-            for(Waypoint distWP : NewWaypointList) {
+          const auto WpTransform = map->ComputeTransform(Wp);
-                Waypoints.emplace_back(distWP);
+          RoutePlanner = GetWorld()->SpawnActor<ARoutePlanner>();
-                AlreadyVisited.emplace_back(distWP);
+          RoutePlanner->bIsIntersection = map->IsJunction(Wp.road_id);
-            }
+          RoutePlanner->SetBoxExtent(FVector(70.f, 70.f, 50.f));
-            Dist += RoadAccuracy;
+          RoutePlanner->SetActorRotation(WpTransform.rotation);
-          }
+          RoutePlanner->SetActorLocation(WpTransform.location +
-          auto NewWaypointList = map->GetNext(Successor, MaxDist);
+              FVector(0.f, 0.f, TriggersHeight));
-          // if((map->IsJunction(Successor.road_id) && NewWaypointList.size()>1) || (!map->IsJunction(Successor.road_id) && NewWaypointList.size()==1))
+        }
-          for(Waypoint distWP : NewWaypointList) {
-            Waypoints.emplace_back(NewWaypointList[0]);
-           }
 
-          if(Waypoints.size() >= 2) {
+        if (RoutePlanner != nullptr)
-
+        {
-            TArray<FVector> Positions;
+          RoutePlanner->AddRoute(1.f, Positions);
-            Positions.Reserve(Waypoints.size());
+          RoutePlanners.Add(RoutePlanner);
-            for (int i = 0; i < Waypoints.size(); ++i)
+        }
-            {
-              // Add the trigger height because the z position of the points does not
-              // influence on the driver AI and is easy to visualize in the editor
-              Positions.Add(map->ComputeTransform(Waypoints[i]).location +
-                  FVector(0.f, 0.f, TriggersHeight));
-            }
-
-            // If the route planner does not exist, create it
-            if (RoutePlanner == nullptr )
-            {
-              RoutePlanner = GetWorld()->SpawnActor<ARoutePlanner>();
-              RoutePlanner->bIsIntersection = std::any_of(Successors.begin(), Successors.end(), [&map](auto w) {
-                return map->IsJunction(w.road_id);
-              });
-              RoutePlanner->SetBoxExtent(FVector(70.f, 70.f, 50.f));
-              RoutePlanner->SetActorRotation(map->ComputeTransform(Successor).rotation);
-              RoutePlanner->SetActorLocation(map->ComputeTransform(Successor).location +
-                  FVector(0.f, 0.f, TriggersHeight));
-            }
-
-            if(RoutePlanner!=nullptr) RoutePlanner->AddRoute(1.f, Positions);
-            if(RoutePlanner!=nullptr) RoutePlanners.Add(RoutePlanner);
-          }
       }
     }
   }
-
-  // const std::vector<TrafficGroup> TrafficLightGroup = map.GetTrafficGroups();
-  // for (TrafficGroup CurrentTrafficLightGroup : TrafficLightGroup)
-  // {
-  //   double RedTime = CurrentTrafficLightGroup.red_time;
-  //   double YellowTime = CurrentTrafficLightGroup.yellow_time;
-  //   double GreenTime = CurrentTrafficLightGroup.green_time;
-  //   FActorSpawnParameters SpawnParams;
-  //   FOutputDeviceNull ar;
-  //   AActor *SpawnedTrafficGroup = GetWorld()->SpawnActor<AActor>(TrafficGroupBlueprintClass,
-  //       FVector(0, 0, 0),
-  //       FRotator(0, 0, 0),
-  //       SpawnParams);
-  //   FString SetTrafficTimesCommand = FString::Printf(TEXT("SetTrafficTimes %f %f %f"),
-  //           RedTime, YellowTime, GreenTime);
-  //   SpawnedTrafficGroup->CallFunctionByNameWithArguments(*SetTrafficTimesCommand, ar, NULL, true);
-  //   for (TrafficLight CurrentTrafficLight : CurrentTrafficLightGroup.traffic_lights)
-  //   {
-  //     FVector TLPos =
-  //         FVector(CurrentTrafficLight.x_pos, CurrentTrafficLight.y_pos, CurrentTrafficLight.z_pos);
-  //     FRotator TLRot = FRotator(CurrentTrafficLight.x_rot,
-  //         CurrentTrafficLight.z_rot,
-  //         CurrentTrafficLight.y_rot);
-  //     AActor *SpawnedTrafficLight = GetWorld()->SpawnActor<AActor>(TrafficLightBlueprintClass,
-  //         TLPos,
-  //         TLRot,
-  //         SpawnParams);
-  //     FString AddTrafficLightCommand = FString::Printf(TEXT("AddTrafficLightPole %s"),
-  //           *SpawnedTrafficLight->GetName());
-  //     SpawnedTrafficGroup->CallFunctionByNameWithArguments(*AddTrafficLightCommand, ar, NULL, true);
-  //     PersistentTrafficLights.Push(SpawnedTrafficGroup);
-  //     SpawnedTrafficLight->CallFunctionByNameWithArguments(TEXT("InitData"), ar, NULL, true);
-  //     for (TrafficBoxComponent TfBoxComponent : CurrentTrafficLight.box_areas)
-  //     {
-  //       FVector TLBoxPos = FVector(TfBoxComponent.x_pos,
-  //           TfBoxComponent.y_pos,
-  //           TfBoxComponent.z_pos);
-  //       FRotator TLBoxRot = FRotator(TfBoxComponent.x_rot,
-  //           TfBoxComponent.z_rot,
-  //           TfBoxComponent.y_rot);
-
-  //       FString BoxCommand = FString::Printf(TEXT("SetBoxLocationAndRotation %f %f %f %f %f %f"),
-  //           TLBoxPos.X,
-  //           TLBoxPos.Y,
-  //           TLBoxPos.Z,
-  //           TLBoxRot.Pitch,
-  //           TLBoxRot.Roll,
-  //           TLBoxRot.Yaw);
-  //       SpawnedTrafficLight->CallFunctionByNameWithArguments(*BoxCommand, ar, NULL, true);
-  //       PersistentTrafficLights.Push(SpawnedTrafficLight);
-  //     }
-  //   }
-  // }
-
-  // const std::vector<TrafficSign> TrafficSigns = map.GetTrafficSigns();
-  // for (TrafficSign CurrentTrafficSign : TrafficSigns)
-  // {
-  //   //switch()
-  //   AActor* SignActor;
-  //   FOutputDeviceNull ar;
-
-  //   FVector TSLoc = FVector(CurrentTrafficSign.x_pos, CurrentTrafficSign.y_pos, CurrentTrafficSign.z_pos);
-  //   FRotator TSRot = FRotator(CurrentTrafficSign.x_rot, CurrentTrafficSign.z_rot, CurrentTrafficSign.y_rot);
-  //   FActorSpawnParameters SpawnParams;
-  //   switch(CurrentTrafficSign.speed) {
-  //     case 30:
-  //       SignActor = GetWorld()->SpawnActor<AActor>(TrafficSign30BlueprintClass,
-  //         TSLoc,
-  //         TSRot,
-  //         SpawnParams);
-  //       break;
-  //     case 60:
-  //       SignActor = GetWorld()->SpawnActor<AActor>(TrafficSign60BlueprintClass,
-  //         TSLoc,
-  //         TSRot,
-  //         SpawnParams);
-  //       break;
-  //     case 90:
-  //       SignActor = GetWorld()->SpawnActor<AActor>(TrafficSign90BlueprintClass,
-  //         TSLoc,
-  //         TSRot,
-  //         SpawnParams);
-  //       break;
-  //     case 100:
-  //       SignActor = GetWorld()->SpawnActor<AActor>(TrafficSign100BlueprintClass,
-  //         TSLoc,
-  //         TSRot,
-  //         SpawnParams);
-  //         break;
-  //     default:
-  //       FString errorMessage = "Traffic Sign not found. Posibilities: 30, 60, 90, 100";
-  //       UE_LOG(LogCarla, Warning, TEXT("%s"), *errorMessage);
-  //     break;
-  //   }
-  //   PersistentTrafficSigns.Push(SignActor);
-  //   for (TrafficBoxComponent TfBoxComponent : CurrentTrafficSign.box_areas)
-  //   {
-  //     FVector TLBoxPos = FVector(TfBoxComponent.x_pos,
-  //         TfBoxComponent.y_pos,
-  //         TfBoxComponent.z_pos);
-  //     FRotator TLBoxRot = FRotator(TfBoxComponent.x_rot,
-  //         TfBoxComponent.z_rot,
-  //         TfBoxComponent.y_rot);
-
-  //     FString BoxCommand = FString::Printf(TEXT("SetBoxLocationAndRotation %f %f %f %f %f %f"),
-  //         TLBoxPos.X,
-  //         TLBoxPos.Y,
-  //         TLBoxPos.Z,
-  //         TLBoxRot.Pitch,
-  //         TLBoxRot.Roll,
-  //         TLBoxRot.Yaw);
-  //     SignActor->CallFunctionByNameWithArguments(*BoxCommand, ar, NULL, true);
-  //   }
-  // }
 }
 
 void AOpenDriveActor::RemoveRoutes()
@@ -417,22 +244,6 @@ void AOpenDriveActor::RemoveRoutes()
     }
   }
   RoutePlanners.Empty();
-  const int tl_num = PersistentTrafficLights.Num();
-  for (int i = 0; i < tl_num; i++)
-  {
-    if(PersistentTrafficLights[i] != nullptr) {
-      PersistentTrafficLights[i]->Destroy();
-    }
-  }
-  PersistentTrafficLights.Empty();
-  const int ts_num = PersistentTrafficSigns.Num();
-  for (int i = 0; i < ts_num; i++)
-  {
-    if(PersistentTrafficSigns[i] != nullptr) {
-      PersistentTrafficSigns[i]->Destroy();
-    }
-  }
-  PersistentTrafficSigns.Empty();
 }
 
 void AOpenDriveActor::DebugRoutes() const
@@ -450,7 +261,7 @@ void AOpenDriveActor::RemoveDebugRoutes() const
 {
 #if WITH_EDITOR
   FlushPersistentDebugLines(GetWorld());
-#endif   // WITH_EDITOR
+#endif // WITH_EDITOR
 }
 
 void AOpenDriveActor::AddSpawners()

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/OpenDriveActor.h

@@ -39,33 +39,6 @@ private:
   UPROPERTY()
   TArray<AVehicleSpawnPoint *> VehicleSpawners;
 
-  UPROPERTY()
-  TArray<AActor *> PersistentTrafficLights;
-
-  UPROPERTY()
-  TArray<AActor *> PersistentTrafficSigns;
-
-  UPROPERTY()
-  TSubclassOf<class AActor> TrafficLightBlueprintClass;
-
-  UPROPERTY()
-  TSubclassOf<class AActor> TrafficGroupBlueprintClass;
-
-  UPROPERTY()
-  TSubclassOf<class AActor> TrafficSign30BlueprintClass;
-
-  UPROPERTY()
-  TSubclassOf<class AActor> TrafficSign40BlueprintClass;
-
-  UPROPERTY()
-  TSubclassOf<class AActor> TrafficSign60BlueprintClass;
-
-  UPROPERTY()
-  TSubclassOf<class AActor> TrafficSign90BlueprintClass;
-
-  UPROPERTY()
-  TSubclassOf<class AActor> TrafficSign100BlueprintClass;
-
 #if WITH_EDITORONLY_DATA
   /// Generate the road network using an OpenDrive file (named as the current
   /// .umap)