diff --git a/Source/Carla/AI/AICarlaVehicleController.cpp b/Source/Carla/AI/AICarlaVehicleController.cpp new file mode 100644 index 000000000..e5226edc4 --- /dev/null +++ b/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 + +#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 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(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 BoundingBoxes; + WheeledVehicle->GetComponents(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(VehicleBounds, TEXT("Right")); + if(rightSphere) + { + rightSphere->RegisterComponent(); + rightSphere->SetupAttachment(RootComponent); + //rightSphere->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepRelativeTransform); + } + + USphereComponent* leftSphere = NewObject(VehicleBounds, TEXT("Left")); + if(leftSphere) + { + leftSphere->RegisterComponent(); + leftSphere->SetupAttachment(RootComponent); + //leftSphere->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepRelativeTransform); + } + + + //CompClass can be a BP + //USphereComponent* SphereVisual = ConstructObject(USphereComponent::StaticClass, this, name); + USphereComponent* rightSphere = GetOwner() -> CreateDefaultSubobject(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(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 ControlPoints; + WheeledVehicle->GetComponents(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 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 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; + } +*/ \ No newline at end of file diff --git a/Source/Carla/AI/AICarlaVehicleController.h b/Source/Carla/AI/AICarlaVehicleController.h new file mode 100644 index 000000000..11519986a --- /dev/null +++ b/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 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 route; + +}; diff --git a/Source/Carla/AI/TrafficLight.cpp b/Source/Carla/AI/TrafficLight.cpp new file mode 100644 index 000000000..43356e9f5 --- /dev/null +++ b/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(); +} + diff --git a/Source/Carla/AI/TrafficLight.h b/Source/Carla/AI/TrafficLight.h new file mode 100644 index 000000000..f6279ddec --- /dev/null +++ b/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; + +}; diff --git a/Source/Carla/AI/TrafficLightGroup.cpp b/Source/Carla/AI/TrafficLightGroup.cpp new file mode 100644 index 000000000..684ed5258 --- /dev/null +++ b/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); + +} + diff --git a/Source/Carla/AI/TrafficLightGroup.h b/Source/Carla/AI/TrafficLightGroup.h new file mode 100644 index 000000000..e521ffe40 --- /dev/null +++ b/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 LightComponents; + +}; diff --git a/Source/Carla/AI/VehicleSpawnerBase.cpp b/Source/Carla/AI/VehicleSpawnerBase.cpp new file mode 100644 index 000000000..364418dd5 --- /dev/null +++ b/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 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); +} \ No newline at end of file diff --git a/Source/Carla/AI/VehicleSpawnerBase.h b/Source/Carla/AI/VehicleSpawnerBase.h new file mode 100644 index 000000000..efc273438 --- /dev/null +++ b/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 SpawnPoints; +/* + UPROPERTY(Category = "Vechicle Spawner", BlueprintReadOnly, EditAnywhere, AdvancedDisplay) + TArray Vehicles; +*/ + UPROPERTY(Category = "Vehicle Spawner", BlueprintReadOnly, VisibleAnywhere, AdvancedDisplay) + TArray< AWheeledVehicle *> Vehicles; + + +}; diff --git a/Source/Carla/Game/CarlaGameController.cpp b/Source/Carla/Game/CarlaGameController.cpp index 8b15ffce1..6bfd4bcfa 100644 --- a/Source/Carla/Game/CarlaGameController.cpp +++ b/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 diff --git a/Source/Carla/MapGen/IntersectionEntrance.cpp b/Source/Carla/MapGen/IntersectionEntrance.cpp new file mode 100644 index 000000000..464ccc2e8 --- /dev/null +++ b/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 AIntersectionEntrance::GetRoute(int it) +{ + TArray points = Routes[it].points; + TArray 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();//USphereComponent* createdComp = NewObject(this);//CreateDefaultSubobject(TEXT("Sphere")); + USceneComponent* SphereMesh = NewObject(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 +*/ + diff --git a/Source/Carla/MapGen/IntersectionEntrance.h b/Source/Carla/MapGen/IntersectionEntrance.h new file mode 100644 index 000000000..a3d92048c --- /dev/null +++ b/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 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; + +}; diff --git a/Source/Carla/MapGen/RoadMap.cpp b/Source/Carla/MapGen/RoadMap.cpp index a51914e07..b60fa43cf 100644 --- a/Source/Carla/MapGen/RoadMap.cpp +++ b/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: diff --git a/Source/CarlaServer/source/carla/CarlaServer.cpp b/Source/CarlaServer/source/carla/CarlaServer.cpp index b35f4f1de..18b08965a 100644 --- a/Source/CarlaServer/source/carla/CarlaServer.cpp +++ b/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; } diff --git a/Source/CarlaServer/source/carla/CarlaServer.h b/Source/CarlaServer/source/carla/CarlaServer.h index 52af5aa40..9f010ac98 100644 --- a/Source/CarlaServer/source/carla/CarlaServer.h +++ b/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 possible_positions; /// Projection matrices of the cameras. - std::vector> projection_matrices; + //std::vector> 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. /// diff --git a/Source/CarlaServer/source/carla/server/CarlaCommunication.cpp b/Source/CarlaServer/source/carla/server/CarlaCommunication.cpp index dae9080a7..48d7954ed 100644 --- a/Source/CarlaServer/source/carla/server/CarlaCommunication.cpp +++ b/Source/CarlaServer/source/carla/server/CarlaCommunication.cpp @@ -82,8 +82,7 @@ namespace server { static std::unique_ptr worldReceiveThread(TCPServer &server, thread::AsyncReadWriteJobQueue &thr) { auto message = std::make_unique(); 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 values) { _serverThread.push(std::move(values)); } - bool CarlaCommunication::tryReadControl(float &steer, float &gas) { - steer = 0.0f; - gas = 0.0f; + bool CarlaCommunication::tryReadControl(Control_Values &control_values) { + control_values.steer = 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(); - gas = control.gas(); + control_values.steer = control.steer(); + 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 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() { diff --git a/Source/CarlaServer/source/carla/server/CarlaCommunication.h b/Source/CarlaServer/source/carla/server/CarlaCommunication.h index d8111cee9..2d90f750f 100644 --- a/Source/CarlaServer/source/carla/server/CarlaCommunication.h +++ b/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 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(); diff --git a/Source/CarlaServer/source/carla/server/Protocol.cpp b/Source/CarlaServer/source/carla/server/Protocol.cpp index fe37a8561..b45b6fbc6 100644 --- a/Source/CarlaServer/source/carla/server/Protocol.cpp +++ b/Source/CarlaServer/source/carla/server/Protocol.cpp @@ -11,12 +11,7 @@ #include #ifdef CARLA_WITH_PNG_COMPRESSION -#include CARLA_LIBPNG_INCLUDE -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"); +#include #endif // CARLA_WITH_PNG_COMPRESSION namespace carla { @@ -204,26 +199,17 @@ namespace server { static bool getPNGImages(const std::vector &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 &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 &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 &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 &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 &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); } - +*/ } } diff --git a/Source/CarlaServer/source/carla/server/TCPServer.cpp b/Source/CarlaServer/source/carla/server/TCPServer.cpp index 2999ce480..dea5e0ed8 100644 --- a/Source/CarlaServer/source/carla/server/TCPServer.cpp +++ b/Source/CarlaServer/source/carla/server/TCPServer.cpp @@ -43,7 +43,11 @@ namespace carla { } - TCPServer::~TCPServer() {} + TCPServer::~TCPServer() { + + close(); + + } void TCPServer::AcceptSocket() { diff --git a/Source/CarlaServer/source/carla/server/carla_protocol.proto b/Source/CarlaServer/source/carla/server/carla_protocol.proto index 103816d69..2fcefde0d 100644 --- a/Source/CarlaServer/source/carla/server/carla_protocol.proto +++ b/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 bytes positions = 2; - optional bytes projection_matrices = 3; + //optional int32 number_of_cameras = 1; + optional bytes positions = 1; + //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 images = 19; - optional bytes depths = 20; + optional bytes final_image_sizes = 18; + optional bytes depth_sizes = 19; + optional bytes semantic_seg_sizes = 20; + optional bytes images = 21; + optional bytes final_images = 22; + optional bytes depths = 23; + optional bytes semantic_segs = 24; } diff --git a/Source/CarlaServer/source/carla/thread/AsyncReadWriteJobQueue.h b/Source/CarlaServer/source/carla/thread/AsyncReadWriteJobQueue.h index 82ee5cfb1..537140f60 100644 --- a/Source/CarlaServer/source/carla/thread/AsyncReadWriteJobQueue.h +++ b/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 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) { - auto value = _readQueue.wait_and_pop(); + while (!_restart && !done) { + /* 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; diff --git a/Source/CarlaServer/source/carla/thread/AsyncReaderJobQueue.h b/Source/CarlaServer/source/carla/thread/AsyncReaderJobQueue.h index 9635e2c7e..279c79758 100644 --- a/Source/CarlaServer/source/carla/thread/AsyncReaderJobQueue.h +++ b/Source/CarlaServer/source/carla/thread/AsyncReaderJobQueue.h @@ -24,7 +24,7 @@ namespace thread { using ReconnectJob = std::function; 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 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; diff --git a/Source/CarlaServer/source/carla/thread/AsyncWriterJobQueue.h b/Source/CarlaServer/source/carla/thread/AsyncWriterJobQueue.h index c23479cb8..938656809 100644 --- a/Source/CarlaServer/source/carla/thread/AsyncWriterJobQueue.h +++ b/Source/CarlaServer/source/carla/thread/AsyncWriterJobQueue.h @@ -22,7 +22,7 @@ namespace thread { using ReconnectJob = std::function; 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 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; diff --git a/Source/CarlaServer/source/carla/thread/ThreadSafeQueue.h b/Source/CarlaServer/source/carla/thread/ThreadSafeQueue.h index 3418447c8..6d1648802 100644 --- a/Source/CarlaServer/source/carla/thread/ThreadSafeQueue.h +++ b/Source/CarlaServer/source/carla/thread/ThreadSafeQueue.h @@ -47,9 +47,11 @@ namespace thread { std::unique_ptr wait_and_pop() { std::unique_lock lock(_mutex); + _condition.wait(lock, [this]() { return _value != nullptr || !_canWait; }); + return std::move(_value); } diff --git a/Source/CarlaServer/source/test/async_server.cpp b/Source/CarlaServer/source/test/async_server.cpp index 7ee8a5b7a..b33212709 100644 --- a/Source/CarlaServer/source/test/async_server.cpp +++ b/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 (;;) { - if (server.init(1u)) { + std::string file; + 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 pMatrix = {{ 10.0 }}; - for (int i = 0; i < 100; ++i) sceneValues.projection_matrices.push_back(pMatrix); + //const std::array pMatrix = {{ 10.0 }}; + //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; - bool newEpisode = false; - if (!server.newEpisodeRequested(newEpisode)){ + //float steer, gas; + bool readed = false; + 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