From 2bc3b397cb885b4b2548e428be9881be2e6c5e36 Mon Sep 17 00:00:00 2001
From: dotero <daniel.oterop@gmail.com>
Date: Fri, 15 Nov 2019 12:17:59 +0100
Subject: [PATCH] Added missed Set function to Gnss and fixed Set function in
 BlueprintLibrary

---
 .../Actor/ActorBlueprintFunctionLibrary.cpp   | 10 ++++-----
 .../Carla/Source/Carla/Sensor/GnssSensor.cpp  | 21 ++++++++++++++++---
 .../Carla/Source/Carla/Sensor/GnssSensor.h    |  2 ++
 3 files changed, 25 insertions(+), 8 deletions(-)

diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp
index 9bffad7eb..ff5af411c 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp
@@ -1280,13 +1280,13 @@ void UActorBlueprintFunctionLibrary::SetGnss(
   Gnss->SetLatitudeDeviation(
       RetrieveActorAttributeToFloat("noise_lat_stddev", Description.Variations, 0.0f));
   Gnss->SetLongitudeDeviation(
-      RetrieveActorAttributeToFloat("noise_lat_bias", Description.Variations, 0.0f));
-  Gnss->SetAltitudeDeviation(
       RetrieveActorAttributeToFloat("noise_long_stddev", Description.Variations, 0.0f));
-  Gnss->SetLatitudeBias(
-      RetrieveActorAttributeToFloat("noise_long_bias", Description.Variations, 0.0f));
-  Gnss->SetLongitudeBias(
+  Gnss->SetAltitudeDeviation(
       RetrieveActorAttributeToFloat("noise_alt_stddev", Description.Variations, 0.0f));
+  Gnss->SetLatitudeBias(
+      RetrieveActorAttributeToFloat("noise_lat_bias", Description.Variations, 0.0f));
+  Gnss->SetLongitudeBias(
+      RetrieveActorAttributeToFloat("noise_long_bias", Description.Variations, 0.0f));
   Gnss->SetAltitudeBias(
       RetrieveActorAttributeToFloat("noise_alt_bias", Description.Variations, 0.0f));
 }
diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.cpp
index 3c7521e89..3387c9582 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.cpp
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.cpp
@@ -24,6 +24,14 @@ FActorDefinition AGnssSensor::GetSensorDefinition()
   return UActorBlueprintFunctionLibrary::MakeGnssDefinition();
 }
 
+void AGnssSensor::Set(const FActorDescription &ActorDescription)
+{
+  Super::Set(ActorDescription);
+  // Fill the parameters that the user requested
+  // Not currently needed in this sensor
+  UActorBlueprintFunctionLibrary::SetGnss(ActorDescription, this);
+}
+
 void AGnssSensor::Tick(float DeltaSeconds)
 {
   Super::Tick(DeltaSeconds);
@@ -33,9 +41,16 @@ void AGnssSensor::Tick(float DeltaSeconds)
   carla::geom::GeoLocation current_location = CurrentGeoLocation.Transform(location);
 
   auto Stream = GetDataStream(*this);
-  double latitude = current_location.latitude + LatitudeBias + RandomEngine->GetNormalDistribution(0.0f, LatitudeDeviation);
-  double longitude = current_location.longitude + LongitudeBias + RandomEngine->GetNormalDistribution(0.0f, LongitudeDeviation);
-  double altitude = current_location.altitude + AltitudeBias + RandomEngine->GetNormalDistribution(0.0f, AltitudeDeviation);;
+  float lat_error = RandomEngine->GetNormalDistribution(0.0f, LatitudeDeviation);
+  float long_error = RandomEngine->GetNormalDistribution(0.0f, LongitudeDeviation);
+  float alt_error = RandomEngine->GetNormalDistribution(0.0f, AltitudeDeviation);
+
+  UE_LOG(LogCarla, Warning, TEXT("lat = %f - long = %f - alt = %f"), lat_error, long_error, alt_error);
+  UE_LOG(LogCarla, Warning, TEXT("lat_dev = %f - long_dev = %f - alt_dev = %f"), LatitudeDeviation, LongitudeDeviation, AltitudeDeviation);
+
+  double latitude = current_location.latitude + LatitudeBias + lat_error;
+  double longitude = current_location.longitude + LongitudeBias + long_error;
+  double altitude = current_location.altitude + AltitudeBias + alt_error;
   Stream.Send(*this, carla::geom::GeoLocation{latitude, longitude, altitude});
 
 }
diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.h
index a72305979..090f96e62 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.h
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.h
@@ -30,6 +30,8 @@ public:
 
   static FActorDefinition GetSensorDefinition();
 
+  void Set(const FActorDescription &ActorDescription);
+
   void Tick(float DeltaSeconds) override;
 
   void SetLatitudeDeviation(float Value);