Merge branch 'xisco' into nestor

Source/Carla/AI/AICarlaVehicleController.cpp

@@ -0,0 +1,433 @@
+// CARLA, Copyright (C) 2017 Computer Vision Center (CVC)
+
+#include "Carla.h"
+#include "AICarlaVehicleController.h"
+
+#include "Components/BoxComponent.h"
+#include "Components/SphereComponent.h"
+#include "Kismet/KismetMathLibrary.h"
+#include "WheeledVehicle.h"
+#include "WheeledVehicleMovementComponent.h"
+#include "EngineUtils.h"
+#include "GameFramework/Pawn.h"
+#include "CityMapGenerator.h"
+#include "Tagger.h"
+#include "TrafficLight.h"
+#include "math.h"
+#include <DrawDebugHelpers.h>
+
+#include "MapGen/RoadMap.h"
+
+
+// Find first component of type road.
+static bool RayTrace(
+    UWorld *World,
+    const FVector &Start,
+    const FVector &End,
+    bool &Stop)
+{
+
+  FHitResult Hit;
+  TArray <FHitResult> OutHits;
+  static FName TraceTag = FName(TEXT("VehicleTrace"));
+  
+  //World->DebugDrawTraceTag = TraceTag;
+  
+  const bool Success = World->LineTraceMultiByObjectType(
+        OutHits,
+        Start,
+        End,
+        FCollisionObjectQueryParams(ECollisionChannel::ECC_Vehicle),
+        FCollisionQueryParams(TraceTag, true));
+
+
+  if (Success) {
+    for (FHitResult &Item : OutHits) {
+      if (ATagger::MatchComponent(*Item.Component, ECityObjectLabel::Vehicles)) {
+        Stop = true;
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
+
+
+AAICarlaVehicleController::AAICarlaVehicleController() : 
+  Super(),
+  MovementComponent(nullptr)
+{
+  bAutoManageActiveCameraTarget = false;
+  //MAX_SPEED = ((rand() * 10) - 5) + MAX_SPEED; 
+}
+
+AAICarlaVehicleController::~AAICarlaVehicleController() {}
+
+// =============================================================================
+// -- APlayerController --------------------------------------------------------
+// =============================================================================
+
+void AAICarlaVehicleController::SetupInputComponent(){ Super::SetupInputComponent(); }
+
+void AAICarlaVehicleController::Possess(APawn *aPawn)
+{
+  Super::Possess(aPawn);
+  
+  if (IsPossessingAVehicle()) {
+    UE_LOG(LogCarla, Error, TEXT("Controller already possessing a pawn!"));
+    return;
+  }
+
+  auto *WheeledVehicle = Cast<AWheeledVehicle>(aPawn);
+  if (WheeledVehicle != nullptr) {
+
+    // Bind hit events.
+    // aPawn->OnActorHit.AddDynamic(this, &AAICarlaVehicleController::OnCollisionEvent);
+    // Get vehicle movement component.
+    MovementComponent = WheeledVehicle->GetVehicleMovementComponent();
+    check(MovementComponent != nullptr);
+
+    // Get vehicle box component.
+    TArray<UBoxComponent *> BoundingBoxes;
+    WheeledVehicle->GetComponents<UBoxComponent>(BoundingBoxes);
+
+    if (BoundingBoxes.Num() > 0) {
+      VehicleBounds = BoundingBoxes[0];
+    } else {
+      UE_LOG(LogCarla, Error, TEXT("Pawn is missing the bounding box!"));
+    }
+
+/*
+    ////////////////////////////////
+    if (VehicleBounds != nullptr){
+      FVector BoxExtent = VehicleBounds->GetScaledBoxExtent();
+
+
+       USphereComponent* rightSphere = NewObject<USphereComponent>(VehicleBounds, TEXT("Right"));
+       if(rightSphere)
+       {
+           rightSphere->RegisterComponent();
+           rightSphere->SetupAttachment(RootComponent);
+           //rightSphere->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepRelativeTransform);
+       }
+
+       USphereComponent* leftSphere = NewObject<USphereComponent>(VehicleBounds, TEXT("Left"));
+       if(leftSphere)
+       {
+           leftSphere->RegisterComponent();
+           leftSphere->SetupAttachment(RootComponent);
+           //leftSphere->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepRelativeTransform);
+       }
+
+
+        //CompClass can be a BP
+        //USphereComponent* SphereVisual = ConstructObject<USphereComponent>(USphereComponent::StaticClass, this, name);
+        USphereComponent* rightSphere = GetOwner() -> CreateDefaultSubobject<USphereComponent>(TEXT("Right"));
+        RootComponent = rightSphere;
+
+         //could use different than Root Comp
+      if (rightSphere)
+      {
+          //SphereVisual->SetStaticMesh(SphereVisualAsset.Object);
+          rightSphere->RegisterComponent();
+          rightSphere->SetRelativeLocation(FVector(BoxExtent.X/2.0, 50.0 + BoxExtent.Y/2.0, 0.0));
+          rightSphere->SetWorldScale3D(FVector(1.0f));
+          rightSphere->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepRelativeTransform); 
+      }
+
+      USphereComponent* leftSphere = GetOwner() -> CreateDefaultSubobject<USphereComponent>(TEXT("Left"));
+        RootComponent = leftSphere;
+
+         //could use different than Root Comp
+      if (leftSphere)
+      {
+          //SphereVisual->SetStaticMesh(SphereVisualAsset.Object);
+          leftSphere->RegisterComponent();
+          leftSphere->SetRelativeLocation(FVector(BoxExtent.X/2.0, 50.0 + BoxExtent.Y/2.0, 0.0));
+          leftSphere->SetWorldScale3D(FVector(1.0f));
+          leftSphere->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepRelativeTransform); 
+      }
+
+    }
+///////////////////////////////////
+    TArray<USphereComponent *> ControlPoints;
+    WheeledVehicle->GetComponents<USphereComponent>(ControlPoints);
+    if (ControlPoints.Num() > 0) {
+      if (ControlPoints[0]->GetName().Equals("Right")){
+        VehicleRightControl = ControlPoints[0];
+        VehicleLeftControl = ControlPoints[1];
+      }
+      else{
+        VehicleRightControl = ControlPoints[1];
+        VehicleLeftControl = ControlPoints[0];
+      }
+    }else{
+      UE_LOG(LogCarla, Error, TEXT("Vehicle control point not found!"));
+    }
+*/
+  }
+
+}
+
+void AAICarlaVehicleController::BeginPlay()
+{
+
+  TActorIterator<ACityMapGenerator> It(GetWorld());
+  if (It) {
+    RoadMap = It->GetRoadMap();
+  }
+
+}
+
+
+void AAICarlaVehicleController::Tick(float DeltaTime){
+	Super::PlayerTick(DeltaTime);
+
+  check(MovementComponent != nullptr);
+  
+  if (RoadMap == nullptr) {
+    UE_LOG(LogCarla, Error, TEXT("Controller doesn't have a road map"));
+    return;
+  }
+
+  FRoadMapPixelData roadData = RoadMap->GetDataAt(GetPawn()->GetActorLocation());
+
+
+  float steering = 2.0, throttle = 1.0f;
+
+  if (route.Num() > 0){
+    steering = GoTo(route[route_it]);
+  }
+  else{
+    steering = CalcStreeringValue();
+  }
+
+  const FVector Start = GetPawn()->GetActorLocation() + (GetPawn()->GetActorForwardVector().GetSafeNormal() * 250.0) + FVector(0.0, 0.0, 50.0);
+  const FVector End = Start + GetPawn()->GetActorForwardVector().GetSafeNormal() * 400.0;
+
+
+  auto speed = MovementComponent->GetForwardSpeed() * 0.036f;
+  
+  //RayTrace to detect trafficLights or Vehicles
+  bool stop;
+  auto World = GetWorld();
+  if (TrafficLightStop) throttle = Stop(speed);
+  else {
+    if (RayTrace(World, Start, End, stop)) {
+      if (stop) throttle = Stop(speed);
+      else throttle = Move(speed);
+    }
+    else{
+       throttle = Move(speed);
+    }
+  }
+  
+
+  MovementComponent->SetSteeringInput(steering);
+  MovementComponent->SetThrottleInput(throttle);
+
+}
+
+float AAICarlaVehicleController::GoTo(FVector objective){
+
+
+    UE_LOG(LogCarla, Log,
+        TEXT("it: %i"),
+        route_it
+        );
+
+    float steering = 0;
+    if (objective.Equals(GetPawn()->GetActorLocation(), 80.0f)){
+        ++route_it;
+      if (route_it == route.Num()){
+        route.Empty();
+        route_it = 0;
+        return CalcStreeringValue();
+      }
+    }
+
+
+    FVector direction = objective - GetPawn()->GetActorLocation();
+    direction = direction.GetSafeNormal();
+
+    FVector forward = GetPawn()->GetActorForwardVector();
+
+    float dirAngle = direction.UnitCartesianToSpherical().Y;
+    float actorAngle = forward.UnitCartesianToSpherical().Y;
+
+    dirAngle *= (180.0f/PI);
+    actorAngle *= (180.0/PI);
+
+    float angle = dirAngle - actorAngle;
+
+    if (angle > 180.0f) angle -= 360.0f;
+    else if (angle < -180.0f) angle += 360.0f;
+
+    if (angle < -70.0f) steering = -1.0f;
+    else if (angle > 70.0f) steering = 1.0f;
+    else steering += angle/70.0f;
+
+  return steering;
+}
+
+float AAICarlaVehicleController::CalcStreeringValue(){
+
+    float steering = 0;
+/*
+    if (VehicleRightControl == nullptr || VehicleLeftControl == nullptr){
+      UE_LOG(LogCarla, Error, TEXT("Vehicle control point not found!"));
+      return 0;
+    } 
+*/
+    FVector BoxExtent = VehicleBounds->GetScaledBoxExtent();
+    FVector forward = GetPawn()->GetActorForwardVector();
+
+    FVector rightSensorPosition (BoxExtent.X/2.0f, (BoxExtent.Y/2.0f) + 150.0f, 0.0f);
+    FVector leftSensorPosition (BoxExtent.X/2.0f, -(BoxExtent.Y/2.0f) - 150.0f, 0.0f);
+
+    
+
+
+    float forwardMagnitude = BoxExtent.X/2.0f;
+
+    float Magnitude = (float) sqrt(pow((double)leftSensorPosition.X,2.0) + pow((double)leftSensorPosition.Y,2.0));
+
+    //same for the right and left
+    float offset = FGenericPlatformMath::Acos(forwardMagnitude/Magnitude);
+
+    float actorAngle = forward.UnitCartesianToSpherical().Y;
+    
+    float sinR = FGenericPlatformMath::Sin(actorAngle+offset);
+    float cosR = FGenericPlatformMath::Cos(actorAngle+offset);
+    
+
+    float sinL = FGenericPlatformMath::Sin(actorAngle-offset);
+    float cosL = FGenericPlatformMath::Cos(actorAngle-offset);
+
+    rightSensorPosition.Y = sinR * Magnitude;
+    rightSensorPosition.X = cosR * Magnitude;
+
+    leftSensorPosition.Y = sinL * Magnitude;
+    leftSensorPosition.X = cosL * Magnitude;
+
+    FVector rightPositon = GetPawn()->GetActorLocation() + FVector(rightSensorPosition.X, rightSensorPosition.Y, 0.0f);
+    FVector leftPosition = GetPawn()->GetActorLocation() + FVector(leftSensorPosition.X, leftSensorPosition.Y, 0.0f);
+
+    FRoadMapPixelData rightRoadData = RoadMap->GetDataAt(rightPositon);
+    if (!rightRoadData.IsRoad()) steering -= 0.2f;
+
+    FRoadMapPixelData leftRoadData = RoadMap->GetDataAt(leftPosition);
+    if (!leftRoadData.IsRoad()) steering += 0.2f;
+
+    FRoadMapPixelData roadData = RoadMap->GetDataAt(GetPawn()->GetActorLocation());
+    if (!roadData.IsRoad()){
+      steering = -1;
+    }
+    else if (roadData.HasDirection()){
+
+      FVector direction = roadData.GetDirection();
+      FVector right = rightRoadData.GetDirection();
+      FVector left = leftRoadData.GetDirection();
+
+
+      forward.Z = 0.0f;
+
+      float dirAngle = direction.UnitCartesianToSpherical().Y;
+      float rightAngle = right.UnitCartesianToSpherical().Y;
+      float leftAngle = left.UnitCartesianToSpherical().Y;
+
+      dirAngle *= (180.0f/PI);
+      rightAngle *= (180.0/PI);
+      leftAngle *= (180.0/PI);
+      actorAngle *= (180.0/PI);
+
+      float min = dirAngle - 90.0f;
+      if (min < -180.0f) min = 180.0f + (min + 180.0f);
+
+      float max = dirAngle + 90.0f;
+      if (max > 180.0f) max = -180.0f + (max - 180.0f);
+      
+      if (dirAngle < -90.0 || dirAngle > 90.0){
+        if (rightAngle < min && rightAngle > max) steering -= 0.2f;
+        if (leftAngle < min && leftAngle > max) steering += 0.2f;
+      }
+      else{
+        if (rightAngle < min || rightAngle > max) steering -= 0.2f;
+        if (leftAngle < min || leftAngle > max) steering += 0.2f;
+      }
+
+      float angle = dirAngle - actorAngle;
+
+      if (angle > 180.0f) angle -= 360.0f;
+      else if (angle < -180.0f) angle += 360.0f;
+
+      if (angle < -70.0f) steering = -1.0f;
+      else if (angle > 70.0f) steering = 1.0f;
+      else steering += angle/70.0f;
+
+    }
+
+    return steering;
+}
+
+float AAICarlaVehicleController::Stop(float &speed){
+  if (speed > 0.0f) return -1.0f;
+  else return 0;
+}
+
+float AAICarlaVehicleController::Move(float &speed){ 
+  return  1.0f - (speed/MAX_SPEED);
+}
+
+
+void AAICarlaVehicleController::RedTrafficLight(bool state){
+  if (state) TrafficLightStop = true;
+  else TrafficLightStop = false;
+}
+
+
+void AAICarlaVehicleController::NewSpeedLimit(float speed){
+  MAX_SPEED = speed;
+
+
+
+  UE_LOG(LogCarla, Log,
+        TEXT("New Speed: %f"),
+        MAX_SPEED
+        );
+}
+
+
+void AAICarlaVehicleController::NewRoute(TArray<FVector> positions){
+  this->route = positions;
+  route_it = 0;
+}
+/*
+ bool AAICarlaVehicleController::DoTrace()
+ {
+    FHitResult RV_Hit(ForceInit);
+
+ 
+    FVector Start = GetPawn()->GetActorLocation() + (GetPawn()->GetActorForwardVector() * 250) + FVector(0.0, 0.0, 50.0);
+    
+    // you need to add a uproperty to the header file for a float PlayerInteractionDistance
+    FVector End = Start + (GetPawn()->GetActorForwardVector() * 500);
+ 
+    //If Trace Hits anything
+    if(  UMyStaticFunctionLibrary::Trace(GetWorld(),GetPawn(),Start,End,HitData)  )
+    {
+      //Print out the name of the traced actor
+      if(HitData.GetActor())
+      {
+        ClientMessage(HitData.GetActor()->GetName());
+     
+              //Print out distance from start of trace to impact point
+              ClientMessage("Trace Distance: " + FString::SanitizeFloat(HitData.Distance));
+      }
+      return true;
+    }
+ 
+    return false;
+ }
+*/

Source/Carla/AI/AICarlaVehicleController.h

@@ -0,0 +1,139 @@
+// CARLA, Copyright (C) 2017 Computer Vision Center (CVC)
+
+#pragma once
+
+#include "GameFramework/PlayerController.h"
+#include "AICarlaVehicleController.generated.h"
+//#include "TrafficLight.h"
+
+class UBoxComponent; 
+class USphereComponent; 
+class URoadMap;
+class UWheeledVehicleMovementComponent;
+
+/**
+ * 
+ */
+UCLASS()
+class CARLA_API AAICarlaVehicleController : public APlayerController
+{
+	GENERATED_BODY()
+
+	// ===========================================================================
+  	/// @name Constructor and destructor
+  	// ===========================================================================
+  	/// @{
+public:
+
+  	AAICarlaVehicleController();
+
+  	~AAICarlaVehicleController();
+
+	/// @}
+  	// ===========================================================================
+  	/// @name APlayerController overrides
+  	// ===========================================================================
+  	/// @{
+public:
+
+  	virtual void SetupInputComponent() override;
+
+  	virtual void Possess(APawn *aPawn) override;
+
+  	virtual void BeginPlay() override;
+
+  	virtual void Tick(float DeltaTime) override;
+
+  	/// @}
+  	// ===========================================================================
+  	/// @name Vehicle pawn info
+  	// ===========================================================================
+  	/// @{
+public:
+
+  	bool IsPossessingAVehicle() const
+  	{
+  	  return MovementComponent != nullptr;
+  	}
+
+  	/// World location of the vehicle.
+  	FVector GetVehicleLocation() const;
+
+  	/// Speed forward in km/h. Might be negative if goes backwards.
+  	float GetVehicleForwardSpeed() const;
+
+  	/// Orientation vector of the vehicle, pointing forward.
+  	FVector GetVehicleOrientation() const;
+
+  	int32 GetVehicleCurrentGear() const;
+
+private:
+    void InitVehilceValues();
+
+  	/// @}
+  	// ===========================================================================
+  	/// @name Vehicle movement
+  	// ===========================================================================
+  	/// @{
+public:
+
+  	void SetThrottleInput(float Value);
+
+  	void SetSteeringInput(float Value);
+
+  	void SetHandbrakeInput(bool Value);
+
+    /// @}
+    // ===========================================================================
+    /// @name Blueprint functions
+    // ===========================================================================
+    /// @{
+public:
+
+    UFUNCTION(BlueprintCallable, Category="Trigger")
+    void RedTrafficLight(bool state);
+
+    UFUNCTION(BlueprintCallable, Category="Trigger")
+    void NewSpeedLimit(float speed);
+
+    UFUNCTION(BlueprintCallable, Category="Trigger")
+    void NewRoute(TArray<FVector> positions);
+
+
+private:
+  float CalcStreeringValue();
+  float GoTo(FVector objective);
+  float Stop(float &speed);
+  float Move(float &speed);
+
+  bool DoTrace();
+
+private:
+
+	UPROPERTY()
+  	UBoxComponent *VehicleBounds;
+
+  UPROPERTY()
+    USphereComponent* VehicleRightControl;
+
+  UPROPERTY()
+    USphereComponent* VehicleLeftControl;
+
+  UPROPERTY()
+    AActor *forwardTrigger;
+
+	UPROPERTY()
+  URoadMap *RoadMap;
+
+  UPROPERTY()
+  	UWheeledVehicleMovementComponent *MovementComponent;
+
+  UPROPERTY(EditAnywhere)
+  float MAX_SPEED = 30.0f;
+
+  bool TrafficLightStop = false;
+
+  int route_it = 0;
+  TArray<FVector> route;
+	
+};

Source/Carla/AI/TrafficLight.cpp

@@ -0,0 +1,20 @@
+// CARLA, Copyright (C) 2017 Computer Vision Center (CVC)
+
+#include "Carla.h"
+#include "TrafficLight.h"
+
+
+// Sets default values
+ATrafficLight::ATrafficLight()
+{
+ 	// Set this actor to call Tick() every frame.  You can turn this off to improve performance if you don't need it.
+	PrimaryActorTick.bCanEverTick = true;
+
+}
+
+// Called when the game starts or when spawned
+void ATrafficLight::BeginPlay()
+{
+	Super::BeginPlay();
+}
+

Source/Carla/AI/TrafficLight.h

@@ -0,0 +1,37 @@
+// CARLA, Copyright (C) 2017 Computer Vision Center (CVC)
+
+#pragma once
+
+#include "GameFramework/Actor.h"
+#include "TrafficLight.generated.h"
+
+
+UENUM(BlueprintType)
+	enum class ETrafficLightState : uint8{
+		RED 		UMETA(DisplayName = "Red"),
+		YELLOW		UMETA(DisplayName = "Yellow"),
+		GEEN		UMETA(DisplayName = "Green")
+	};
+
+
+UCLASS()
+class CARLA_API ATrafficLight : public AActor
+{
+
+	GENERATED_BODY()
+	
+public:	
+
+  	ATrafficLight();
+
+  	~ATrafficLight(){};
+
+protected:
+	// Called when the game starts or when spawned
+	virtual void BeginPlay() override;
+
+public:
+	UPROPERTY(BlueprintReadWrite, Category=TrafficLightValues, EditAnywhere)
+ 	ETrafficLightState state;
+	
+};

Source/Carla/AI/TrafficLightGroup.cpp

@@ -0,0 +1,28 @@
+// CARLA, Copyright (C) 2017 Computer Vision Center (CVC)
+
+#include "Carla.h"
+#include "TrafficLightGroup.h"
+
+
+// Sets default values
+ATrafficLightGroup::ATrafficLightGroup()
+{
+ 	// Set this actor to call Tick() every frame.  You can turn this off to improve performance if you don't need it.
+	PrimaryActorTick.bCanEverTick = true;
+
+}
+
+// Called when the game starts or when spawned
+void ATrafficLightGroup::BeginPlay()
+{
+	Super::BeginPlay();
+	
+}
+
+// Called every frame
+void ATrafficLightGroup::Tick(float DeltaTime)
+{
+	Super::Tick(DeltaTime);
+
+}
+

Source/Carla/AI/TrafficLightGroup.h

@@ -0,0 +1,30 @@
+// CARLA, Copyright (C) 2017 Computer Vision Center (CVC)
+
+#pragma once
+
+#include "GameFramework/Actor.h"
+#include "TrafficLightGroup.generated.h"
+
+UCLASS()
+class CARLA_API ATrafficLightGroup : public AActor
+{
+	GENERATED_BODY()
+	
+public:	
+	// Sets default values for this actor's properties
+	ATrafficLightGroup();
+
+protected:
+	// Called when the game starts or when spawned
+	virtual void BeginPlay() override;
+
+public:	
+	// Called every frame
+	virtual void Tick(float DeltaTime) override;
+
+public:
+
+	UPROPERTY (BlueprintReadOnly, Category=TrafficLights, EditAnywhere)
+	TArray<AActor*> LightComponents;
+	
+};

Source/Carla/AI/VehicleSpawnerBase.cpp

@@ -0,0 +1,106 @@
+// CARLA, Copyright (C) 2017 Computer Vision Center (CVC)
+
+#include "Carla.h"
+#include "VehicleSpawnerBase.h"
+
+#include "Engine/PlayerStartPIE.h"
+#include "EngineUtils.h"
+#include "GameFramework/PlayerStart.h"
+#include "GameFramework/Character.h"
+
+// Sets default values
+AVehicleSpawnerBase::AVehicleSpawnerBase(const FObjectInitializer& ObjectInitializer) :
+  Super(ObjectInitializer),
+  RandomStream(Seed)
+{
+  PrimaryActorTick.bCanEverTick = true;
+  PrimaryActorTick.TickGroup = TG_PrePhysics;
+
+}
+
+
+void AVehicleSpawnerBase::BeginPlay()
+{
+  Super::BeginPlay();
+
+  // Allocate space for walkers.
+  Vehicles.Reserve(NumberOfVehicles);
+
+  // Set seed for random numbers.
+  if (!bUseFixedSeed) {
+    RandomStream.GenerateNewSeed();
+  } else {
+    RandomStream.Initialize(Seed);
+  }
+
+  // Find spawn points present in level.
+  for (TActorIterator<APlayerStart> It(GetWorld()); It; ++It) {
+    SpawnPoints.Add(*It);
+  }
+
+  UE_LOG(LogCarla, Log, TEXT("Found %d positions for spawning vehilces"), SpawnPoints.Num());
+
+  if (SpawnPoints.Num() < 2) {
+    bSpawnVehicles = false;
+    UE_LOG(LogCarla, Error, TEXT("We don't have enough spawn points for vehicles!"));
+  }
+
+  while (bSpawnVehicles && (NumberOfVehicles > Vehicles.Num())) {
+    // Try to spawn one walker.
+    TryToSpawnRandomVehicle();
+  }
+}
+
+void AVehicleSpawnerBase::Tick(float DeltaTime)
+{
+  Super::Tick(DeltaTime);
+}
+
+// =============================================================================
+// -- Other member functions ---------------------------------------------------
+// =============================================================================
+
+void AVehicleSpawnerBase::SetNumberOfVehicles(const int32 Count)
+{
+  if (Count > 0) {
+    bSpawnVehicles = true;
+    NumberOfVehicles = Count;
+  } else {
+    bSpawnVehicles = false;
+  }
+}
+
+void AVehicleSpawnerBase::TryToSpawnRandomVehicle()
+{
+  auto SpawnPoint = GetRandomSpawnPoint();
+  if ((SpawnPoint != nullptr)) {
+      SpawnVehicleAtSpawnPoint(*SpawnPoint);
+  } else {
+    UE_LOG(LogCarla, Error, TEXT("Unable to find spawn point"));
+  }
+}
+
+void AVehicleSpawnerBase::SpawnVehicleAtSpawnPoint(
+    const APlayerStart &SpawnPoint)
+{
+  AWheeledVehicle *Vehicle;
+  SpawnVehicle(SpawnPoint.GetActorTransform(), Vehicle);
+  if ((Vehicle != nullptr) && !Vehicle->IsPendingKill()) {
+    Vehicle->AIControllerClass = AAICarlaVehicleController::StaticClass();
+    Vehicle->SpawnDefaultController();
+    auto Controller = GetVehicleController(Vehicle);
+    if (Controller != nullptr) { // Sometimes fails...
+      Vehicles.Add(Vehicle);
+      } else {
+      UE_LOG(LogCarla, Error, TEXT("Something went wrong creating the controller for the new vehicle"));
+      Vehicle->Destroy();
+    }
+  }
+}
+
+APlayerStart *AVehicleSpawnerBase::GetRandomSpawnPoint() const
+{
+  return (SpawnPoints.Num() > 0 ?
+      SpawnPoints[RandomStream.RandRange(0, SpawnPoints.Num() - 1)] :
+      nullptr);
+}

Source/Carla/AI/VehicleSpawnerBase.h

@@ -0,0 +1,90 @@
+// CARLA, Copyright (C) 2017 Computer Vision Center (CVC)
+
+#pragma once
+
+#include "GameFramework/Actor.h"
+#include "VehicleSpawnerBase.generated.h"
+//#include "Content/Blueprints/BaseVehiclePawn.h"
+
+
+class APlayerStart;
+
+UCLASS(Abstract)
+class CARLA_API AVehicleSpawnerBase : public AActor
+{
+	GENERATED_BODY()
+	
+
+public:	
+	// Sets default values for this actor's properties
+	AVehicleSpawnerBase(const FObjectInitializer& ObjectInitializer);
+
+
+
+protected:
+	// Called when the game starts or when spawned
+	virtual void BeginPlay() override;
+
+  UFUNCTION(BlueprintCallable)
+  const FRandomStream &GetRandomStream() const
+  {
+    return RandomStream;
+  }
+
+  UFUNCTION(BlueprintImplementableEvent)
+  void SpawnVehicle(const FTransform &SpawnTransform, AWheeledVehicle *&SpawnedCharacter);
+
+  //UFUNCTION(BlueprintImplementableEvent)
+  void TryToSpawnRandomVehicle();
+
+  UFUNCTION(BlueprintImplementableEvent)
+  AAICarlaVehicleController* GetVehicleController(AWheeledVehicle* Vechicle);
+
+
+
+public:	
+	// Called every frame
+	virtual void Tick(float DeltaTime) override;
+
+   void SetNumberOfVehicles(int32 Count);
+
+   APlayerStart* GetRandomSpawnPoint() const;
+
+   void SpawnVehicleAtSpawnPoint(const APlayerStart &SpawnPoint);
+
+protected:
+
+/** If false, no walker will be spawned. */
+  UPROPERTY(Category = "Vehicle Spawner", EditAnywhere)
+  bool bSpawnVehicles = true;
+
+  /** Number of walkers to be present within the volume. */
+  UPROPERTY(Category = "Vehicle Spawner", EditAnywhere, meta = (EditCondition = bSpawnVehicles, ClampMin = "1"))
+  int32 NumberOfVehicles = 10;
+
+  /** Minimum walk distance in centimeters. */
+  /*UPROPERTY(Category = "Vechicle Spawner", EditAnywhere, meta = (EditCondition = bSpawnWalkers))
+  float MinimumWalkDistance = 1500.0f;
+*/
+  /** If false, a random seed is generated each time. */
+  UPROPERTY(Category = "Vehicle Spawner", EditAnywhere, meta = (EditCondition = bSpawnVehicles))
+  bool bUseFixedSeed = true;
+
+  /** Seed for spawning random walkers. */
+  UPROPERTY(Category = "Vehicle Spawner", EditAnywhere, meta = (EditCondition = bUseFixedSeed))
+  int32 Seed = 123456789;
+
+  UPROPERTY()
+  FRandomStream RandomStream;
+
+  //UPROPERTY(Category = "Vechicle Spawner", VisibleAnywhere, AdvancedDisplay)
+  TArray<APlayerStart *> SpawnPoints;
+/*
+  UPROPERTY(Category = "Vechicle Spawner", BlueprintReadOnly, EditAnywhere, AdvancedDisplay)
+  TArray<AWheeledVehicle *> Vehicles;
+*/
+  UPROPERTY(Category = "Vehicle Spawner", BlueprintReadOnly, VisibleAnywhere, AdvancedDisplay)
+  TArray< AWheeledVehicle *> Vehicles;
+	
+	
+};

Source/Carla/Game/CarlaGameController.cpp

@@ -76,10 +76,10 @@ static bool ReadSceneInit(carla::CarlaServer &Server)
   uint32 Scene;
   bool Success = false;
   UE_LOG(LogCarlaServer, Log, TEXT("(tryReadSceneInit) Waiting for client..."));
-  while (!Success) {
+  /*while (!Success) {
     if (!Server.tryReadSceneInit(Scene, Success))
       return false;
-  }
+  }*/
   return true;
 }
 
@@ -188,11 +188,11 @@ void CarlaGameController::Initialize(UCarlaSettings &CarlaSettings)
 {
   if (bServerNeedsRestart) {
     UE_LOG(LogCarlaServer, Log, TEXT("Initializing CarlaServer"));
-    if (Server->init(1u) && ReadSceneInit(*Server)) {
+    /*if (Server->init(1u) && ReadSceneInit(*Server)) {
       bServerNeedsRestart = false;
     } else {
       UE_LOG(LogCarlaServer, Warning, TEXT("Failed to initialize, server needs restart"));
-    }
+    }*/
   }
 }
 
@@ -238,13 +238,13 @@ bool CarlaGameController::TickServer()
 {
   // Check if the client requested a new episode.
   bool bNewEpisodeRequested = false;
-  if (!Server->newEpisodeRequested(bNewEpisodeRequested)) {
+  /*if (!Server->newEpisodeRequested(bNewEpisodeRequested)) {
     return false;
   } else if (bNewEpisodeRequested) {
     UE_LOG(LogCarlaServer, Log, TEXT("New episode requested"));
     RestartLevel();
     return true;
-  }
+  }*/
 
   // Send reward and try to read control.
   return

Source/Carla/MapGen/IntersectionEntrance.cpp

@@ -0,0 +1,68 @@
+// CARLA, Copyright (C) 2017 Computer Vision Center (CVC)
+
+#include "Carla.h"
+#include "IntersectionEntrance.h"
+
+
+// Sets default values
+AIntersectionEntrance::AIntersectionEntrance(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
+{
+ 	// Set this actor to call Tick() every frame.  You can turn this off to improve performance if you don't need it.
+	PrimaryActorTick.bCanEverTick = true;
+}
+
+// Called when the game starts or when spawned
+void AIntersectionEntrance::BeginPlay()
+{
+	Super::BeginPlay();
+	
+}
+
+// Called every frame
+void AIntersectionEntrance::Tick(float DeltaTime)
+{
+	Super::Tick(DeltaTime);
+
+}
+
+TArray<FVector> AIntersectionEntrance::GetRoute(int it)
+{
+	TArray<AActor*> points = Routes[it].points;
+	TArray<FVector> route;
+
+	for (int i = 0; i < points.Num(); ++i){
+		route.Add(points[i]->GetActorLocation());
+	}
+
+	return route;
+}
+
+/*
+#if WITH_EDITOR
+
+void AIntersectionEntrance::PostEditChangeProperty(FPropertyChangedEvent& PropertyChangedEvent)
+{
+  Super::PostEditChangeProperty(PropertyChangedEvent);
+  if (PropertyChangedEvent.Property) {
+    if (bCreateRoutes && (GetWorld() != nullptr)) {
+    	//ClearRoutes();
+	    for (int i = 0; i < Routes.Num(); ++i){
+	      	for(int e = 0; e < Routes[i].points.Num(); ++e){
+	      		AActor* actor= GetWorld()->SpawnActor<AActor>();//USphereComponent* createdComp = NewObject<USphereComponent>(this);//CreateDefaultSubobject<USphereComponent>(TEXT("Sphere"));
+				USceneComponent* SphereMesh = NewObject<USceneComponent>(actor);
+     			SphereMesh->AttachToComponent(RootComponent,FAttachmentTransformRules::KeepWorldTransform);
+					if(actor)
+					{
+					   actor->RegisterAllComponents();
+					   Routes[i].points[e] = actor;
+					   //Routes[i].points[e].position = createdComp->GetRelativeTransform().GetLocation();
+					}
+	      	}
+	    }
+    }
+  }
+  bCreateRoutes = false;
+}
+#endif // WITH_EDITOR
+*/
+

Source/Carla/MapGen/IntersectionEntrance.h

@@ -0,0 +1,53 @@
+// CARLA, Copyright (C) 2017 Computer Vision Center (CVC)
+
+#pragma once
+
+#include "GameFramework/Actor.h"
+#include "IntersectionEntrance.generated.h"
+
+USTRUCT(BlueprintType)
+struct FRoute {
+
+	GENERATED_BODY()
+
+	UPROPERTY(BlueprintReadWrite, Category=TrafficRoutes, EditAnywhere)
+	TArray < AActor *> points;
+};
+
+
+UCLASS(BlueprintType)
+class CARLA_API AIntersectionEntrance : public AActor
+{
+	GENERATED_BODY()
+	
+public:	
+	// Sets default values for this actor's properties
+	AIntersectionEntrance(const FObjectInitializer& ObjectInitializer);
+
+protected:
+	// Called when the game starts or when spawned
+	virtual void BeginPlay() override;
+
+public:	
+	// Called every frame
+	virtual void Tick(float DeltaTime) override;
+
+protected:
+
+UFUNCTION(BlueprintCallable, Category="Trigger")
+    TArray<FVector> GetRoute(int route);
+/*
+#if WITH_EDITOR
+  virtual void PostEditChangeProperty(FPropertyChangedEvent& PropertyChangedEvent) override;
+#endif // WITH_EDITOR
+*/
+public:
+
+	UPROPERTY(Category = "Routes", EditAnywhere)
+  	bool bCreateRoutes = false;
+
+
+	UPROPERTY(BlueprintReadWrite, Category=TrafficRoutes, EditAnywhere)
+	TArray< FRoute > Routes;
+	
+};

Source/Carla/MapGen/RoadMap.cpp

@@ -135,7 +135,7 @@ void URoadMap::SetPixelAt(
     case ECityMapMeshTag::Road90DegTurn_Lane7:
       bIsRoad = true;
       bHasDirection = true;
-      Rotator.Yaw += 90.0f + 22.5f;
+      Rotator.Yaw += 90.0f; //+ 15.5f;
       break;
     case ECityMapMeshTag::Road90DegTurn_Lane5:
       bIsRoad = true;
@@ -145,7 +145,7 @@ void URoadMap::SetPixelAt(
     case ECityMapMeshTag::Road90DegTurn_Lane3:
       bIsRoad = true;
       bHasDirection = true;
-      Rotator.Yaw += 90.0f + 45.0f + 22.5f;
+      Rotator.Yaw += 90.0f + 45.0f + 20.5f;
       break;
     case ECityMapMeshTag::Road90DegTurn_Lane8:
     case ECityMapMeshTag::RoadTIntersection_Lane4:
@@ -158,17 +158,17 @@ void URoadMap::SetPixelAt(
     case ECityMapMeshTag::Road90DegTurn_Lane6:
       bIsRoad = true;
       bHasDirection = true;
-      Rotator.Yaw += 270.0f + 22.5f;
+      Rotator.Yaw += 270.0f + 30.0f;
       break;
     case ECityMapMeshTag::Road90DegTurn_Lane4:
       bIsRoad = true;
       bHasDirection = true;
-      Rotator.Yaw += 270.0f + 45.0f;
+      Rotator.Yaw += 270.0f + 50.0f;
       break;
     case ECityMapMeshTag::Road90DegTurn_Lane2:
       bIsRoad = true;
       bHasDirection = true;
-      Rotator.Yaw += 270.0f + 45.0f + 22.5f;
+      Rotator.Yaw += 270.0f + 70.0f;
       break;
     case ECityMapMeshTag::RoadTIntersection_Lane3:
     case ECityMapMeshTag::RoadTIntersection_Lane6:

Source/CarlaServer/source/carla/CarlaServer.cpp

@@ -30,6 +30,7 @@ namespace carla {
 
   CarlaServer::~CarlaServer() {}
 
+/*
   bool CarlaServer::init(uint32_t levelCount) {
     if (!worldConnected() && !clientConnected() && !serverConnected())
       return false;
@@ -43,6 +44,7 @@ namespace carla {
     readed = _pimpl->communication.tryReadSceneInit(scene);
     return true;
   }
+*/
 
   bool CarlaServer::tryReadEpisodeStart(uint32_t &startIndex, uint32_t &endIndex, bool &readed) {
     if (!worldConnected())
@@ -51,17 +53,19 @@ namespace carla {
     return true;
   }
 
-  bool CarlaServer::tryReadControl(float &steer, float &throttle, bool &readed) {
+  bool CarlaServer::tryReadControl(Control_Values &control, bool &readed) {
     if (!clientConnected())
       return false;
-    readed = _pimpl->communication.tryReadControl(steer, throttle);
+    readed = _pimpl->communication.tryReadControl(control);
     return true;
   }
 
-  bool CarlaServer::newEpisodeRequested(bool &newEpisode) {
+  bool CarlaServer::newEpisodeRequested(std::string &init_file, bool &readed) {
     if (!worldConnected())
       return false;
-    newEpisode = _pimpl->communication.tryReadRequestNewEpisode();
+    
+    readed = _pimpl->communication.tryReadRequestNewEpisode(init_file);
+
     return true;
   }
 

Source/CarlaServer/source/carla/CarlaServer.h

@@ -29,7 +29,9 @@ namespace carla {
 
   enum ImageType {
     IMAGE,
+    SCENE_FINAL,
     DEPTH,
+    SEMANTIC_SEG,
   };
 
   struct Image {
@@ -40,6 +42,15 @@ namespace carla {
     uint32_t width, height;
   };
 
+  struct Control_Values {
+
+    float steer;
+    float gas;
+    float brake;
+    float hand_brake;
+    float gear;
+  };
+
   struct Reward_Values {
     Reward_Values();
     ~Reward_Values();
@@ -76,7 +87,7 @@ namespace carla {
     /// Possible world positions to spawn the player.
     std::vector<Vector2D> possible_positions;
     /// Projection matrices of the cameras.
-    std::vector<std::array<float, 16u>> projection_matrices;
+    //std::vector<std::array<float, 16u>> projection_matrices;
   };
 
   /// Asynchronous TCP server. Uses three ports, one for sending messages
@@ -100,11 +111,11 @@ namespace carla {
     /// Initialize the server.
     ///
     /// @param LevelCount Number of levels available.
-    bool init(uint32_t levelCount);
+    //bool init(uint32_t levelCount);
 
     /// Try to read if the client has selected an scene and mode. Return false
     /// if the queue is empty.
-    bool tryReadSceneInit(uint32_t &scene, bool &readed);
+    //bool tryReadSceneInit(uint32_t &scene, bool &readed);
 
     /// Try to read if the client has selected an end & start point. Return
     /// false if the queue is empty.
@@ -112,9 +123,9 @@ namespace carla {
 
     /// Try to read the response of the client. Return false if the queue
     /// is empty.
-    bool tryReadControl(float &steer, float &throttle, bool &readed);
+    bool tryReadControl(Control_Values &control, bool &readed);
 
-    bool newEpisodeRequested(bool &newEpisode);
+    bool newEpisodeRequested(std::string &init_file, bool &readed);
 
     /// Send values of the current player status.
     ///

Source/CarlaServer/source/carla/server/CarlaCommunication.cpp

@@ -82,8 +82,7 @@ namespace server {
   static std::unique_ptr<std::string> worldReceiveThread(TCPServer &server, thread::AsyncReadWriteJobQueue<std::string, std::string> &thr) {
     auto message = std::make_unique<std::string>();
     bool success = false;
-
+    
-
       if (!thr.getRestart()) {
         TCPServer::error_code error;
 
@@ -185,13 +184,23 @@ namespace server {
       [this]() { ReconnectAll(*this); }
     } {}
 
+
+  CarlaCommunication::~CarlaCommunication(){
+    _worldThread.done = true;
+    _serverThread.done = true;
+    _clientThread.done = true;
+  }
+
   void CarlaCommunication::sendReward(std::unique_ptr<Reward_Values> values) {
     _serverThread.push(std::move(values));
   }
 
-  bool CarlaCommunication::tryReadControl(float &steer, float &gas) {
+  bool CarlaCommunication::tryReadControl(Control_Values &control_values) {
-    steer = 0.0f;
+    control_values.steer = 0.0f;
-    gas = 0.0f;
+    control_values.gas = 0.0f;
+    control_values.brake = 0.0f;
+    control_values.hand_brake = 0.0f;
+    control_values.gear = 0.0f;
 
     auto message = _clientThread.tryPop();
     if (message == nullptr) { return false; }
@@ -199,12 +208,15 @@ namespace server {
     Control control;
     if (!control.ParseFromString(*message)) { return false; }
 
-    steer = control.steer();
+    control_values.steer = control.steer();
-    gas = control.gas();
+    control_values.gas = control.gas();
+    control_values.brake = control.brake();
+    control_values.hand_brake = control.hand_brake();
+    control_values.gear = control.gear();
 
     return true;
   }
-
+/*
   void CarlaCommunication::sendWorld(const uint32_t scenes) {
     //ClearThreads
     _worldThread.clear();
@@ -220,7 +232,7 @@ namespace server {
       _worldThread.push(std::move(message));
     }
   }
-
+*/
   void CarlaCommunication::sendScene(const Scene_Values &values) {
 
     Scene scene;
@@ -243,6 +255,7 @@ namespace server {
     }
   }
 
+/*
   bool CarlaCommunication::tryReadSceneInit(uint32_t &scene) {
     scene = 0u;
 
@@ -256,6 +269,7 @@ namespace server {
     scene = sceneInit.scene();
     return true;
   }
+*/
 
   bool CarlaCommunication::tryReadEpisodeStart(uint32_t &start_index, uint32_t &end_index) {
     start_index = 0;
@@ -273,17 +287,28 @@ namespace server {
     return true;
   }
 
-  bool CarlaCommunication::tryReadRequestNewEpisode() {
+  bool CarlaCommunication::tryReadRequestNewEpisode(std::string &init_file) {
     std::unique_ptr<std::string> request = _worldThread.tryPop();
 
-    if (request == nullptr) { return false; }
+    if (request == nullptr) { 
+      return false; 
+    }
 
     RequestNewEpisode reqEpisode;
 
     if (!reqEpisode.ParseFromString(*request)) {
+
       _worldThread.undoPop(std::move(request));
+
       return false;
-    } else { return true; }
+    } else { 
+      
+      init_file = reqEpisode.init_file();
+
+      std::cout << init_file << std::endl;
+
+      return true; 
+    }
   }
 
   void CarlaCommunication::restartServer() {

Source/CarlaServer/source/carla/server/CarlaCommunication.h

@@ -17,6 +17,7 @@ namespace carla {
 
     struct Scene_Values;
     struct Reward_Values;
+    struct Control_Values;
     enum class Mode : int8_t;
 
 namespace server {
@@ -26,23 +27,23 @@ namespace server {
 
     explicit CarlaCommunication(int worldPort, int writePort, int readPort);
 
-    //~CarlaCommunication();
+    ~CarlaCommunication();
 
     void sendReward(std::unique_ptr<Reward_Values> values);
 
-    bool tryReadControl(float &steer, float &gas);
+    bool tryReadControl(Control_Values &control);
 
     void sendScene(const Scene_Values &scene);
 
     void sendReset();
 
-    void sendWorld(const uint32_t scenes);
+    //void sendWorld(const uint32_t scenes);
 
-    bool tryReadSceneInit(uint32_t &scene);
+    //bool tryReadSceneInit(uint32_t &scene);
 
     bool tryReadEpisodeStart(uint32_t &start_index, uint32_t &end_index);
 
-    bool tryReadRequestNewEpisode();
+    bool tryReadRequestNewEpisode(std::string &init_file);
 
     void restartServer();
 

Source/CarlaServer/source/carla/server/Protocol.cpp

@@ -11,12 +11,7 @@
 #include <stdlib.h>
 
 #ifdef CARLA_WITH_PNG_COMPRESSION
-#include CARLA_LIBPNG_INCLUDE
+#include <png.h>
-static_assert(
-        CARLA_LIBPNG_VERSION_MAJOR == PNG_LIBPNG_VER_MAJOR &&
-        CARLA_LIBPNG_VERSION_MINOR == PNG_LIBPNG_VER_MINOR &&
-        CARLA_LIBPNG_VERSION_RELEASE == PNG_LIBPNG_VER_RELEASE,
-    "PNG versions do not match");
 #endif // CARLA_WITH_PNG_COMPRESSION
 
 namespace carla {
@@ -204,26 +199,17 @@ namespace server {
 static bool getPNGImages(const std::vector<Image> &images, Reward &rwd) {
 
     std::string image_data;
+    std::string sceneFinal_data;
     std::string depth_data;
+    std::string semanticSeg_data;
     std::string image_size_data;
+    std::string sceneFinal_size_data;
     std::string depth_size_data;
+    std::string semanticSeg_size_data;
 
     for (const Image &img : images){
       img_encode compressedImage;
 
-      /*
-      {
-      using namespace std;
-      clock_t begin = clock();
-      if (!GetImage(img, compressedImage)) {
-        std::cerr << "Error while encoding image" << std::endl;
-        return false;
-      }
-      clock_t end = clock();
-      double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
-      cout << "Time to encode the image: " << elapsed_secs << " sec" << endl;
-      }*/
-
 
       if (!GetImage(img, compressedImage)) {
         std::cerr << "Error while encoding image" << std::endl;
@@ -231,14 +217,26 @@ static bool getPNGImages(const std::vector<Image> &images, Reward &rwd) {
       }
 
       switch (img.type){
+
         case IMAGE:
           for (unsigned long int i = 0; i < compressedImage.size; ++i) image_data += compressedImage.buffer[i];
           image_size_data += GetBytes(compressedImage.size);
           break;
+
+        case SCENE_FINAL:
+          for (unsigned long int i = 0; i < compressedImage.size; ++i) sceneFinal_data += compressedImage.buffer[i];
+          sceneFinal_size_data += GetBytes(compressedImage.size);
+          break;
+
         case DEPTH:
           for (unsigned long int i = 0; i < compressedImage.size; ++i) depth_data += compressedImage.buffer[i];
           depth_size_data += GetBytes(compressedImage.size);
           break;
+
+        case SEMANTIC_SEG:
+          for (unsigned long int i = 0; i < compressedImage.size; ++i) semanticSeg_data += compressedImage.buffer[i];
+          semanticSeg_size_data += GetBytes(compressedImage.size);
+          break;
       }
 
       free(compressedImage.buffer);
@@ -246,9 +244,13 @@ static bool getPNGImages(const std::vector<Image> &images, Reward &rwd) {
     }
 
     rwd.set_depth_sizes(depth_size_data);
+    rwd.set_final_image_sizes(sceneFinal_size_data);
     rwd.set_image_sizes(image_size_data);
+    rwd.set_semantic_seg_sizes(semanticSeg_size_data);
     rwd.set_images(image_data);
+    rwd.set_final_images(sceneFinal_data);
     rwd.set_depths(depth_data);
+    rwd.set_semantic_segs(semanticSeg_data);
 
     return true;
   }
@@ -292,7 +294,7 @@ static bool getPNGImages(const std::vector<Image> &images, Reward &rwd) {
 
   void Protocol::LoadScene(Scene &scene, const Scene_Values &values) {
 
-    scene.set_number_of_cameras(values.projection_matrices.size());
+    //scene.set_number_of_cameras(values.projection_matrices.size());
 
     std::string positions_bytes = "";
 
@@ -316,7 +318,7 @@ static bool getPNGImages(const std::vector<Image> &images, Reward &rwd) {
 
     scene.set_positions(positions_bytes);
 
-    std::string matrices;
+   /* std::string matrices;
 
     for (auto i = 0u; i < values.projection_matrices.size(); ++i) {
       for (auto e = 0u; e < 16u; ++e){
@@ -330,14 +332,15 @@ static bool getPNGImages(const std::vector<Image> &images, Reward &rwd) {
       }
     }
 
-    scene.set_projection_matrices(matrices);
+    scene.set_projection_matrices(matrices);*/
 
   }
 
+/*
   void Protocol::LoadWorld(World &world, const int modes, const int scenes) {
     world.set_modes(modes);
     world.set_scenes(scenes);
   }
-
+*/
 }
 }

Source/CarlaServer/source/carla/server/TCPServer.cpp

@@ -43,7 +43,11 @@ namespace carla {
 
             }
 
-        TCPServer::~TCPServer() {}
+        TCPServer::~TCPServer() {
+
+          close();
+
+        }
 
         void TCPServer::AcceptSocket() {
 

Source/CarlaServer/source/carla/server/carla_protocol.proto

@@ -6,8 +6,7 @@ syntax = "proto2";
 message RequestNewEpisode {
 
 
-  optional bool request =1;
+  optional bytes init_file = 1;
-
 
 }
 
@@ -20,39 +19,27 @@ message EpisodeStart {
 
 }
 
-message SceneInit {
-
-
-  optional int32 mode =1;
-  optional int32 scene =2;
-
-
-}
 
 message Control {
 
   optional float steer = 1;
   optional float gas = 2;
-
+  optional float brake = 3;
+  optional float hand_brake = 4;
+  optional float gear = 5;
 
 }
 
 
 // Server messages
 
-message World {
 
-  optional int32 modes = 1; // the number of modes
-  optional int32 scenes = 2;
-
-
-}
 
 message Scene {
 
-  optional int32 number_of_cameras = 1;
+  //optional int32 number_of_cameras = 1;
-  optional bytes positions = 2;
+  optional bytes positions = 1;
-  optional bytes projection_matrices = 3;
+  //optional bytes projection_matrices = 3;
 
 }
 
@@ -65,6 +52,7 @@ message EpisodeReady{
 
 
 
+
 message Reward {
 
   optional float player_x = 1;
@@ -84,8 +72,12 @@ message Reward {
   optional float ori_y = 15;
   optional float ori_z = 16;
   optional bytes image_sizes = 17;
-  optional bytes depth_sizes = 18;
+  optional bytes final_image_sizes = 18;
-  optional bytes images = 19;
+  optional bytes depth_sizes = 19;
-  optional bytes depths = 20;
+  optional bytes semantic_seg_sizes = 20;
+  optional bytes images = 21;
+  optional bytes final_images = 22;
+  optional bytes depths = 23;
+  optional bytes semantic_segs = 24;
 
 }

Source/CarlaServer/source/carla/thread/AsyncReadWriteJobQueue.h

@@ -28,7 +28,7 @@ namespace thread {
         ReadingJob && readingJob,
         ConnectJob && connectJob,
         ReconnectJob && reconnectJob) :
-      _done(false),
+      done(false),
       _restart(true),
       _writeJob(std::move(writingJob)),
       _readJob(std::move(readingJob)),
@@ -39,7 +39,7 @@ namespace thread {
 
     ~AsyncReadWriteJobQueue() {
       std::cout << "Destroyed thread world"<< std::endl;
-      _done = true;
+      done = true;
     }
 
     std::unique_ptr<W>  tryPop() {
@@ -76,29 +76,36 @@ namespace thread {
       _writeQueue.clear();
     }
 
+    std::atomic_bool done;
+
   private:
 
     void workerThread() {
-      while(!_done){
+      while(!done){
         _connectJob();
         _restart = false; 
         _readQueue.canWait(true);
-        while (!_restart && !_done) {
+        while (!_restart && !done) {
-          auto value = _readQueue.wait_and_pop();
+      /* auto value = _readQueue.wait_and_pop();
           if (value != nullptr) {
             _readJob(*value);
           }
-
+*/
           if (!_restart){
             //_writeQueue.wait_and_push(_writeJob);
             _writeQueue.push(std::move(_writeJob()));
           }
 
+          auto value = _readQueue.wait_and_pop();
+          if (value != nullptr) {
+            _readJob(*value);
+          }
+
         }
       }
     }
 
-    std::atomic_bool _done;
+    
     std::atomic_bool _restart;
 
     WritingJob _writeJob;

Source/CarlaServer/source/carla/thread/AsyncReaderJobQueue.h

@@ -24,7 +24,7 @@ namespace thread {
   using ReconnectJob = std::function<void()>;
 
 	explicit AsyncReaderJobQueue(Job &&job, ConnectJob &&connectionJob, ReconnectJob &&reconnectJob) :
-		_done(false),
+		done(false),
     _restart(true),
 		_job(std::move(job)),
 		_connectionJob(std::move(connectionJob)),
@@ -34,7 +34,7 @@ namespace thread {
 
     ~AsyncReaderJobQueue() {
       std::cout << "Destroyed thread client"<< std::endl;
-      _done = true;
+      done = true;
     }
 
     void push(std::unique_ptr<T> item) {
@@ -58,14 +58,16 @@ namespace thread {
     void clear(){
       _queue.clear();
     }
+
+    std::atomic_bool done;
     
   private:
     void workerThread() {
-      while (!_done){
+      while (!done){
     		_connectionJob();
         _restart = false;
         _queue.canWait(true);
-        while (!_restart && !_done) {
+        while (!_restart && !done) {
           auto value = _queue.wait_and_pop();
           if (value != nullptr) {
             _job(*value);
@@ -75,7 +77,7 @@ namespace thread {
       }
     }
 
-    std::atomic_bool _done;
+    
     std::atomic_bool _restart;
 
     Job _job;

Source/CarlaServer/source/carla/thread/AsyncWriterJobQueue.h

@@ -22,7 +22,7 @@ namespace thread {
   using ReconnectJob = std::function<void()>;
 
 	explicit AsyncWriterJobQueue(Job &&job, ConnectJob &&connectJob, ReconnectJob &&reconnectJob) :
-		_done(false),
+		done(false),
     _restart(true),
 		_job(std::move(job)),
 		_connectJob(std::move(connectJob)),
@@ -33,7 +33,7 @@ namespace thread {
 
     ~AsyncWriterJobQueue() {
       std::cout << "Destroyed thread server"<< std::endl;
-      _done = true;
+      done = true;
     }
 
     std::unique_ptr<T> tryPop() {
@@ -57,14 +57,18 @@ namespace thread {
       _queue.clear();
     }
     
+
+    std::atomic_bool done;
+
   private:
 
     void workerThread() {
-      while (!_done){
+      while (!done){
 		    _connectJob();
         _restart = false;
         _queue.canWait(true);
-        while (!_restart && !_done) {
+
+        while (!_restart && !done) {
           //_queue.wait_and_push(_job);
           _queue.push(std::move(_job())); 
 		      //Sleep(10);
@@ -72,7 +76,7 @@ namespace thread {
       }
     }
 
-    std::atomic_bool _done;
+    
     std::atomic_bool _restart;
 
     Job _job;

Source/CarlaServer/source/carla/thread/ThreadSafeQueue.h

@@ -47,9 +47,11 @@ namespace thread {
 
     std::unique_ptr<T> wait_and_pop() {
       std::unique_lock<std::mutex> lock(_mutex);
+    
       _condition.wait(lock, [this]() {
         return _value != nullptr || !_canWait;
       });
+    
       return std::move(_value);
     }
 

Source/CarlaServer/source/test/async_server.cpp

@@ -97,9 +97,14 @@ int main(int argc, char *argv[]) {
     carla::CarlaServer server(writePort, readPort, worldPort);
 
     // Let's simulate the game loop.
-    for (;;) {
+    std::string file;
-      if (server.init(1u)) {
+    bool read;
 
+
+    for (;;) {
+      if (server.newEpisodeRequested(file, read) && read) {
+
+/*
         {
           uint32_t scene;
           bool error = false, readed = false;
@@ -112,7 +117,7 @@ int main(int argc, char *argv[]) {
           }
           else std::cout << "Received: scene = " << scene << std::endl;
         }
-
+*/
         carla::Scene_Values sceneValues;
 
         for (int i = 0; i < 1; ++i){
@@ -121,8 +126,8 @@ int main(int argc, char *argv[]) {
           sceneValues.possible_positions.push_back({3.0f, 4.0f});
         }
 
-        const std::array<float, 16u> pMatrix = {{ 10.0 }};
+        //const std::array<float, 16u> pMatrix = {{ 10.0 }};
-        for (int i = 0; i < 100; ++i) sceneValues.projection_matrices.push_back(pMatrix);
+        //for (int i = 0; i < 100; ++i) sceneValues.projection_matrices.push_back(pMatrix);
 
         if (!server.sendSceneValues(sceneValues)) {
           std::cerr << "ERROR while sending SceneValues" << std::endl;
@@ -148,14 +153,15 @@ int main(int argc, char *argv[]) {
         }
 
         while (true) {
-          float steer, gas;
+          //float steer, gas;
-          bool newEpisode = false;
+          bool readed = false;
-          if (!server.newEpisodeRequested(newEpisode)){
+          std::string newConfigFile;
+          if (!server.newEpisodeRequested(newConfigFile, readed)){
             std::cerr << "ERROR while checking for newEpisode request" << std::endl;
             break;
           }
 
-          if (newEpisode){
+          if (readed){
             std::cout << "-------- NEW EPISODE --------" << std::endl;
             if (!server.sendSceneValues(sceneValues)){
               std::cerr << "ERROR while sending SceneValues" << std::endl;
@@ -180,11 +186,13 @@ int main(int argc, char *argv[]) {
           } else {
 
             bool readed = false;
-            if (!server.tryReadControl(steer, gas, readed)){
+            carla::Control_Values control;
+            if (!server.tryReadControl(control, readed)){
               std::cerr << "ERROR while reading Control" << std::endl;
               break;
             } else if (readed) {
-              std::cout << "CONTROL -->  gas: " << gas << " steer: " << steer << std::endl;
+              std::cout << "CONTROL -->  gas: " << control.gas << " steer: " << control.steer << 
+              " brake: " << control.brake << " hand_brake: " << control.hand_brake << " gear: " << control.gear << std::endl;
             }      
 
               if (!server.sendReward(makeReward().release())) {
@@ -206,6 +214,3 @@ int main(int argc, char *argv[]) {
   }
 }
 
-//TODO:
-//pmatrix float 16
-//start_index size_t