Merge branch 'nestor' into xisco

Source/Carla/Game/CarlaGameController.cpp

@@ -48,6 +48,36 @@ static void Set(std::vector<carla::Color> &cImage, const TArray<FColor> &BitMap)
 // -- Other static methods -----------------------------------------------------
 // =============================================================================
 
+#ifdef WITH_EDITOR
+
+static bool CheckImage(
+  const FString &Tag,
+  const ACarlaPlayerState::Image &Image,
+  const std::vector<carla::Color> &ImageReward,
+  uint32 SizeX,
+  uint32 SizeY)
+{
+  const auto size = Image.BitMap.Num();
+  UE_LOG(LogCarlaServer, Log, TEXT("PlayerState.%s %dx%d size %d"), *Tag, Image.SizeX, Image.SizeY, size);
+  UE_LOG(LogCarlaServer, Log, TEXT("Reward.%s size %d"), *Tag, ImageReward.size());
+  if (size == 0u)
+    return true;
+  return (Image.SizeX == SizeX) && (Image.SizeY == SizeY) && (size == SizeX * SizeY) && (size == ImageReward.size());
+}
+
+static bool CheckImageValidity(const ACarlaPlayerState &Player, const carla::Reward_Values &Reward)
+{
+  using CPS = ACarlaPlayerState;
+  const uint32 SizeX = Player.GetImage(CPS::ImageRGB0).SizeX;
+  const uint32 SizeY = Player.GetImage(CPS::ImageRGB0).SizeY;
+  return CheckImage("ImageRGB0", Player.GetImage(CPS::ImageRGB0), Reward.image_rgb_0, SizeX, SizeY) &&
+         CheckImage("ImageRGB1", Player.GetImage(CPS::ImageRGB1), Reward.image_rgb_1, SizeX, SizeY) &&
+         CheckImage("ImageDepth0", Player.GetImage(CPS::ImageDepth0), Reward.image_depth_0, SizeX, SizeY) &&
+         CheckImage("ImageDepth1", Player.GetImage(CPS::ImageDepth1), Reward.image_depth_1, SizeX, SizeY);
+}
+
+#endif // WITH_EDITOR
+
 // Wait for the scene init to be sent, return false if we need to restart the
 // server.
 /// @todo At the moment we just ignored what it is sent.
@@ -136,6 +166,9 @@ static bool SendReward(
       Set(reward->image_depth_1, PlayerState.GetImage(CPS::ImageDepth1).BitMap);
     }
   }
+#ifdef WITH_EDITOR
+  check(CheckImageValidity(PlayerState, *reward));
+#endif // WITH_EDITOR
   UE_LOG(LogCarlaServer, Log, TEXT("Sending reward"));
   return Server.sendReward(reward.release());
 }

Source/Carla/Game/CarlaGameMode.cpp

@@ -11,6 +11,7 @@
 
 #include "CarlaGameInstance.h"
 #include "CarlaGameState.h"
+#include "CarlaHUD.h"
 #include "CarlaPlayerState.h"
 #include "CarlaVehicleController.h"
 
@@ -25,6 +26,7 @@ ACarlaGameMode::ACarlaGameMode() :
   PlayerControllerClass = ACarlaVehicleController::StaticClass();
   GameStateClass = ACarlaGameState::StaticClass();
   PlayerStateClass = ACarlaPlayerState::StaticClass();
+  HUDClass = ACarlaHUD::StaticClass();
 }
 
 void ACarlaGameMode::InitGame(

Source/Carla/Game/CarlaHUD.cpp

@@ -0,0 +1,123 @@
+// CARLA, Copyright (C) 2017 Computer Vision Center (CVC)
+
+#include "Carla.h"
+#include "CarlaHUD.h"
+
+#include "CarlaVehicleController.h"
+#include "ConstructorHelpers.h"
+#include "Engine/Canvas.h"
+#include "Engine/Font.h"
+
+#define LOCTEXT_NAMESPACE "CarlaHUD"
+
+static FText RoundedFloatAsText(float Value)
+{
+  return FText::AsNumber(FMath::FloorToInt(Value));
+}
+
+static FText GetVectorAsText(const FVector &Vector)
+{
+  return FText::Format(
+      LOCTEXT("FVectorFormat", "({0}, {1}, {2})"),
+      RoundedFloatAsText(Vector.X),
+      RoundedFloatAsText(Vector.Y),
+      RoundedFloatAsText(Vector.Z));
+}
+
+static FText GetVectorAsText(const FVector &Vector, const FNumberFormattingOptions &FormatOpt)
+{
+  return FText::Format(
+      LOCTEXT("FVectorFormat", "({0}, {1}, {2})"),
+      FText::AsNumber(Vector.X, &FormatOpt),
+      FText::AsNumber(Vector.Y, &FormatOpt),
+      FText::AsNumber(Vector.Z, &FormatOpt));
+}
+
+static FText GetGearAsText(int32 Gear)
+{
+  if (Gear < 0) {
+    return FText(LOCTEXT("ReverseGear", "R"));
+  } else {
+    return (Gear == 0 ? LOCTEXT("NeutralGear", "N") : FText::AsNumber(Gear));
+  }
+}
+
+static FText GetHUDText(const ACarlaPlayerState &Vehicle)
+{
+  // Set number precision.
+  FNumberFormattingOptions HighPrecision;
+  HighPrecision.MinimumFractionalDigits = 2u;
+  HighPrecision.MaximumFractionalDigits = 2u;
+
+  FFormatNamedArguments Args;
+  Args.Add("Location", GetVectorAsText(Vehicle.GetLocation()));
+  Args.Add("Acceleration", GetVectorAsText(Vehicle.GetAcceleration(), HighPrecision));
+  Args.Add("Orientation", GetVectorAsText(Vehicle.GetOrientation(), HighPrecision));
+  Args.Add("Speed", RoundedFloatAsText(Vehicle.GetForwardSpeed()));
+  Args.Add("Gear", GetGearAsText(Vehicle.GetCurrentGear()));
+  Args.Add("CollisionCars", RoundedFloatAsText(Vehicle.GetCollisionIntensityCars()));
+  Args.Add("CollisionPedestrians", RoundedFloatAsText(Vehicle.GetCollisionIntensityPedestrians()));
+  Args.Add("CollisionOther", RoundedFloatAsText(Vehicle.GetCollisionIntensityOther()));
+  Args.Add("IntersectionOtherLane", RoundedFloatAsText(100.0f * Vehicle.GetOtherLaneIntersectionFactor()));
+  Args.Add("IntersectionOffRoad", RoundedFloatAsText(100.0f * Vehicle.GetOffRoadIntersectionFactor()));
+  return FText::Format(
+      LOCTEXT("HUDTextFormat",
+          "Speed:         {Speed} km/h\n"
+          "Gear:          {Gear}\n"
+          "\n"
+          "Location:      {Location}\n"
+          "Acceleration:  {Acceleration}\n"
+          "Orientation:   {Orientation}\n"
+          "\n"
+          "Collision (Cars):       {CollisionCars}\n"
+          "Collision (Pedestrian): {CollisionPedestrians}\n"
+          "Collision (Other):      {CollisionOther}\n"
+          "\n"
+          "Intersection (Lane):    {IntersectionOtherLane}%\n"
+          "Intersection (OffRoad): {IntersectionOffRoad}%"
+      ),
+      Args);
+}
+
+ACarlaHUD::ACarlaHUD()
+{
+  static ConstructorHelpers::FObjectFinder<UFont> Font(TEXT("/Engine/EngineFonts/DroidSansMono"));
+  HUDFont = Font.Object;
+}
+
+void ACarlaHUD::DrawHUD()
+{
+  Super::DrawHUD();
+
+  if (!bIsVisible)
+    return;
+
+  // Calculate ratio from 720p
+  const float HUDXRatio = Canvas->SizeX / 1280.f;
+  const float HUDYRatio = Canvas->SizeY / 720.f;
+
+  ACarlaVehicleController *Vehicle = Cast<ACarlaVehicleController>(
+      GetOwningPawn() == nullptr ? nullptr : GetOwningPawn()->GetController());
+
+  if (Vehicle != nullptr)
+  {
+    FVector2D ScaleVec(HUDYRatio * 1.4f, HUDYRatio * 1.4f);
+
+    auto Text = GetHUDText(Vehicle->GetPlayerState());
+    FCanvasTextItem HUDTextItem(FVector2D(HUDXRatio * 100.0f, HUDYRatio * 400.0f), Text, HUDFont, FLinearColor::White);
+    HUDTextItem.Scale = ScaleVec;
+    Canvas->DrawItem(HUDTextItem);
+  }
+}
+
+void ACarlaHUD::ToggleHUDView()
+{
+  SetVisible(!bIsVisible);
+}
+
+void ACarlaHUD::SetVisible(bool IsVisible)
+{
+  bIsVisible = IsVisible;
+}
+
+#undef LOCTEXT_NAMESPACE

Source/Carla/Game/CarlaHUD.h

@@ -0,0 +1,38 @@
+// CARLA, Copyright (C) 2017 Computer Vision Center (CVC)
+
+#pragma once
+
+#include "GameFramework/HUD.h"
+#include "CarlaHUD.generated.h"
+
+class FTexture;
+
+UCLASS()
+class ACarlaHUD : public AHUD
+{
+  GENERATED_BODY()
+
+public:
+
+  ACarlaHUD();
+
+  UPROPERTY(EditAnywhere)
+  UFont* HUDFont;
+
+  virtual void DrawHUD() override;
+
+  bool IsVisible() const
+  {
+    return bIsVisible;
+  }
+
+  UFUNCTION()
+  void ToggleHUDView();
+
+  UFUNCTION()
+  void SetVisible(bool IsVisible);
+
+private:
+
+  bool bIsVisible = true;
+};

Source/Carla/Game/CarlaPlayerState.h

@@ -73,6 +73,11 @@ public:
     return ForwardSpeed;
   }
 
+  int32 GetCurrentGear() const
+  {
+    return CurrentGear;
+  }
+
   float GetCollisionIntensityCars() const
   {
     return CollisionIntensityCars;
@@ -130,6 +135,8 @@ private:
 
   FVector Acceleration;
 
+  int32 CurrentGear;
+
   float ForwardSpeed = 0.0f;
 
   float CollisionIntensityCars = 0.0f;

Source/Carla/Game/CarlaVehicleController.cpp

@@ -83,10 +83,19 @@ void ACarlaVehicleController::Possess(APawn *aPawn)
     // Get custom player state.
     CarlaPlayerState = Cast<ACarlaPlayerState>(PlayerState);
     check(CarlaPlayerState != nullptr);
+    // Set HUD input.
+    CarlaHUD = Cast<ACarlaHUD>(GetHUD());
+    if (CarlaHUD != nullptr) {
+      InputComponent->BindAction("ToggleHUD", IE_Pressed, CarlaHUD, &ACarlaHUD::ToggleHUDView);
+    } else {
+      UE_LOG(LogCarla, Warning, TEXT("Current HUD is not a ACarlaHUD"));
+    }
   }
 }
 
-static void ReadCameraPixels(const ASceneCaptureCamera *Camera, ACarlaPlayerState::Image &Image)
+static void ReadCameraPixels(
+    const ASceneCaptureCamera *Camera,
+    ACarlaPlayerState::Image &Image)
 {
   if (Camera != nullptr) {
     if (Camera->ReadPixels(Image.BitMap)) {
@@ -110,6 +119,7 @@ void ACarlaVehicleController::Tick(float DeltaTime)
     CarlaPlayerState->ForwardSpeed = GetVehicleForwardSpeed();
     const FVector CurrentSpeed = CarlaPlayerState->ForwardSpeed * CarlaPlayerState->Orientation;
     CarlaPlayerState->Acceleration = (CurrentSpeed - PreviousSpeed) / DeltaTime;
+    CarlaPlayerState->CurrentGear = GetVehicleCurrentGear();
     /// @todo Set intersection factors.
     using CPS = ACarlaPlayerState;
     ReadCameraPixels(RGBCameras[0u], CarlaPlayerState->Images[CPS::ImageRGB0]);
@@ -146,6 +156,12 @@ FVector ACarlaVehicleController::GetVehicleOrientation() const
   return GetPawn()->GetTransform().GetRotation().GetForwardVector();
 }
 
+int32 ACarlaVehicleController::GetVehicleCurrentGear() const
+{
+  check(MovementComponent != nullptr);
+  return MovementComponent->GetCurrentGear();
+}
+
 // =============================================================================
 // -- Scene capture ------------------------------------------------------------
 // =============================================================================

Source/Carla/Game/CarlaVehicleController.h

@@ -5,6 +5,7 @@
 #include "GameFramework/PlayerController.h"
 #include "CarlaVehicleController.generated.h"
 
+class ACarlaHUD;
 class ACarlaPlayerState;
 class ASceneCaptureCamera;
 class UCameraComponent;
@@ -65,6 +66,8 @@ public:
   /// Orientation vector of the vehicle, pointing forward.
   FVector GetVehicleOrientation() const;
 
+  int32 GetVehicleCurrentGear() const;
+
   const ACarlaPlayerState &GetPlayerState() const
   {
     return *CarlaPlayerState;
@@ -176,6 +179,10 @@ private:
   UPROPERTY()
   ACarlaPlayerState *CarlaPlayerState;
 
+  // Cast for quick access to the custom HUD.
+  UPROPERTY()
+  ACarlaHUD *CarlaHUD;
+
   using CaptureCameraArray = std::array<const ASceneCaptureCamera *, 2u>;
 
   CaptureCameraArray RGBCameras;