From ac89115fd9946ba155fdfa571640935e1e1e2f6a Mon Sep 17 00:00:00 2001
From: glopezdiest <58212725+glopezdiest@users.noreply.github.com>
Date: Tue, 5 Nov 2024 11:58:00 +0100
Subject: [PATCH] Added telemetry data (#8335)

---
 .../Carla/Source/Carla/Game/CarlaHUD.cpp      | 42 +++++++++++++++++--
 .../Carla/Source/Carla/Game/CarlaHUD.h        |  3 ++
 .../Carla/Vehicle/CarlaWheeledVehicle.cpp     |  5 ---
 3 files changed, 42 insertions(+), 8 deletions(-)

diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaHUD.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaHUD.cpp
index 5bcabef61..ee39533af 100644
--- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaHUD.cpp
+++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaHUD.cpp
@@ -5,6 +5,10 @@
 // For a copy, see <https://opensource.org/licenses/MIT>.
 
 #include "CarlaHUD.h"
+#include "Engine/Engine.h"
+#include "Engine/Canvas.h"
+#include "CanvasItem.h"
+#include "PhysicalMaterials/PhysicalMaterial.h"
 
 #include "GameFramework/PlayerController.h"
 
@@ -19,13 +23,11 @@ void ACarlaHUD::DrawHUD()
     return;
   }
 
-#if 0 // @CARLAUE5
   if(DebugVehicle) {
     float YL = 1600.0f;
     float Y0 = 0.0f;
-    DebugVehicle->DrawDebug(Canvas, YL, Y0);
+    DrawDebug(YL, Y0);
   }
-#endif
 
   double Now = FPlatformTime::Seconds();
   int i = 0;
@@ -55,3 +57,37 @@ void ACarlaHUD::AddHUDString(const FString Str, const FVector Location, const FC
   HUDString Obj { Str, Location, Color, Now + LifeTime };
   StringList.Add(std::move(Obj));
 }
+
+// Debug
+void ACarlaHUD::DrawDebug(float& YL, float& YPos)
+{
+	using namespace Chaos;
+
+	ensure(IsInGameThread());
+
+	if (!DebugVehicle->HasValidPhysicsState()){
+		UE_LOG(LogCarla, Log, TEXT("Invalid state"));
+		return;
+	}
+
+	UFont* RenderFont = GEngine->GetMediumFont();
+	// draw drive data
+	{
+		Canvas->SetDrawColor(FColor::White);
+	
+		YPos += 16;
+		for (int i = 0; i < DebugVehicle->GetNumWheels(); i++)
+		{
+			YPos += Canvas->DrawText(RenderFont, FString::Printf(TEXT("WheelLoad: [%d] %1.f N"), i, CmToM(DebugVehicle->GetWheelState(i).SpringForce)), 4, YPos);
+		}
+
+		YPos += 16;
+		for (int i = 0; i < DebugVehicle->GetNumWheels(); i++)
+		{
+			if (DebugVehicle->GetWheelState(i).PhysMaterial.IsValid())
+			{
+				YPos += Canvas->DrawText(RenderFont, FString::Printf(TEXT("SurfaceFriction: [%d] %.2f"), i, DebugVehicle->GetWheelState(i).PhysMaterial->Friction), 4, YPos);
+			}
+		}
+	}
+}
diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaHUD.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaHUD.h
index 83b3fbe2f..db80969b1 100644
--- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaHUD.h
+++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaHUD.h
@@ -51,6 +51,9 @@ public:
 
   virtual void DrawHUD() override;
 
+  /** Draw 2D debug text graphs on UI for the wheels, suspension and other systems */
+  void DrawDebug(float& YL, float& YPos);
+
   UChaosWheeledVehicleMovementComponent* DebugVehicle = nullptr;
 
   void AddDebugVehicleForTelemetry(UChaosWheeledVehicleMovementComponent* NewDebugVehiclePtr)
diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp
index 07d4b70e1..dacdad724 100644
--- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp
+++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp
@@ -618,10 +618,6 @@ void ACarlaWheeledVehicle::DeactivateVelocityControl()
 
 void ACarlaWheeledVehicle::ShowDebugTelemetry(bool Enabled)
 {
-  UE_LOG(LogCarla, Warning, TEXT("ACarlaWheeledVehicle::ShowDebugTelemetry:: Chaos does not support Debug telemetry"));
-  return;
-  //To Do: Implement Debug Telemetry?
-  /*
   if (GetWorld()->GetFirstPlayerController())
   {
     ACarlaHUD* hud = Cast<ACarlaHUD>(GetWorld()->GetFirstPlayerController()->GetHUD());
@@ -643,7 +639,6 @@ void ACarlaWheeledVehicle::ShowDebugTelemetry(bool Enabled)
     UE_LOG(LogCarla, Warning, TEXT("ACarlaWheeledVehicle::ShowDebugTelemetry:: Cannot find HUD for debug info"));
     }
   }
-  */
 }
 
 void ACarlaWheeledVehicle::SetVehicleLightState(const FVehicleLightState& LightState)