From 2a11d4528f9ed747f7faaacf73526343b2058491 Mon Sep 17 00:00:00 2001
From: Marc Garcia Puig <marcgpuig@gmail.com>
Date: Thu, 5 Dec 2019 14:32:14 +0100
Subject: [PATCH] Added Radar default noise

---
 .../Actor/ActorBlueprintFunctionLibrary.cpp      | 10 +++++++++-
 .../Plugins/Carla/Source/Carla/Sensor/Radar.cpp  | 16 ++++++++++++++++
 .../Plugins/Carla/Source/Carla/Sensor/Radar.h    |  7 +++++++
 3 files changed, 32 insertions(+), 1 deletion(-)

diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp
index c288ec1a4..b3bd6c04a 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp
@@ -768,7 +768,13 @@ void UActorBlueprintFunctionLibrary::MakeRadarDefinition(
   Aperture.RecommendedValues = { TEXT("10") };
   Aperture.bRestrictToRecommended = false;
 
-  Definition.Variations.Append({FOV, Steps, Far, Aperture});
+  FActorVariation PointLossPercentage;
+  PointLossPercentage.Id = TEXT("point_loss_percentage");
+  PointLossPercentage.Type = EActorAttributeType::Float;
+  PointLossPercentage.RecommendedValues = { TEXT("0.7") };
+  PointLossPercentage.bRestrictToRecommended = false;
+
+  Definition.Variations.Append({FOV, Steps, Far, Aperture, PointLossPercentage});
 
   Success = CheckActorDefinition(Definition);
 }
@@ -1494,6 +1500,8 @@ void UActorBlueprintFunctionLibrary::SetRadar(
     RetrieveActorAttributeToFloat("far", Description.Variations, 100.0f) * TO_CENTIMETERS);
   Radar->SetAperture(
     RetrieveActorAttributeToInt("aperture", Description.Variations, 10));
+  Radar->SetPointLossPercentage(
+    RetrieveActorAttributeToFloat("point_loss_percentage", Description.Variations, 0.7));
 }
 
 #undef CARLA_ABFL_CHECK_ACTOR
diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Radar.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Radar.cpp
index 10708fade..15e5762e1 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Radar.cpp
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Radar.cpp
@@ -19,6 +19,8 @@ ARadar::ARadar(const FObjectInitializer& ObjectInitializer)
 {
   PrimaryActorTick.bCanEverTick = true;
 
+  RandomEngine = CreateDefaultSubobject<URandomEngine>(TEXT("RandomEngine"));
+
   TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this);
   TraceParams.bTraceComplex = true;
   TraceParams.bReturnPhysicalMaterial = false;
@@ -50,6 +52,11 @@ void ARadar::SetAperture(int NewAperture)
   PreCalculateLineTraceIncrement();
 }
 
+void ARadar::SetPointLossPercentage(float NewLossPercentage)
+{
+  PointLossPercentage = NewLossPercentage;
+}
+
 void ARadar::BeginPlay()
 {
   Super::BeginPlay();
@@ -143,6 +150,15 @@ void ARadar::SendLineTraces(float DeltaSeconds)
 
       EndLocation += Rotation * LineTraceIncrement;
 
+      // PointLossPercentage is in range [0.0 - 1.0]
+      // e.g: If PointLossPercentage is 0.7, the 70% of the rays are lost
+      // TODO: Improve the performance precalculating the noise probabilty offline
+      if (RandomEngine->GetBoolWithWeight(PointLossPercentage))
+      {
+        // Do not compute the current ray
+        continue;
+      }
+
       Hitted = World->LineTraceSingleByChannel(
           OutHit,
           RadarLocation,
diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Radar.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Radar.h
index ee86b2029..3c5e66eb5 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Radar.h
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Radar.h
@@ -41,6 +41,9 @@ public:
   UFUNCTION(BlueprintCallable, Category = "Radar")
   void SetAperture(int NewAperture);
 
+  UFUNCTION(BlueprintCallable, Category = "Radar")
+  void SetPointLossPercentage(float NewLossPercentage);
+
 protected:
 
   void BeginPlay() override;
@@ -59,6 +62,10 @@ protected:
   UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category="Detection")
   int Steps;
 
+  /// Noise threshold [0.0, 1.0] of rays that will be
+  UPROPERTY(EditAnywhere, BlueprintReadOnly, Category="Detection")
+  float PointLossPercentage;
+
 private:
 
   void CalculateCurrentVelocity(const float DeltaTime);