Added noise to Gnss

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp

@@ -685,6 +685,69 @@ void UActorBlueprintFunctionLibrary::MakeLidarDefinition(
   Success = CheckActorDefinition(Definition);
 }
 
+FActorDefinition UActorBlueprintFunctionLibrary::MakeGnssDefinition()
+{
+  FActorDefinition Definition;
+  bool Success;
+  MakeGnssDefinition(Success, Definition);
+  check(Success);
+  return Definition;
+}
+
+void UActorBlueprintFunctionLibrary::MakeGnssDefinition(
+    bool &Success,
+    FActorDefinition &Definition)
+{
+  FillIdAndTags(Definition, TEXT("sensor"), TEXT("other"), TEXT("gnss"));
+  AddRecommendedValuesForSensorRoleNames(Definition);
+  AddVariationsForSensor(Definition);
+  // Latitude
+  FActorVariation StdDevLat;
+  StdDevLat.Id = TEXT("noise_lat_stddev");
+  StdDevLat.Type = EActorAttributeType::Float;
+  StdDevLat.RecommendedValues = { TEXT("0.0") };
+  StdDevLat.bRestrictToRecommended = false;
+  FActorVariation BiasLat;
+  BiasLat.Id = TEXT("noise_lat_bias");
+  BiasLat.Type = EActorAttributeType::Float;
+  BiasLat.RecommendedValues = { TEXT("0.0") };
+  BiasLat.bRestrictToRecommended = false;
+
+  // Longitude
+  FActorVariation StdDevLong;
+  StdDevLong.Id = TEXT("noise_long_stddev");
+  StdDevLong.Type = EActorAttributeType::Float;
+  StdDevLong.RecommendedValues = { TEXT("0.0") };
+  StdDevLong.bRestrictToRecommended = false;
+  FActorVariation BiasLong;
+  BiasLong.Id = TEXT("noise_long_bias");
+  BiasLong.Type = EActorAttributeType::Float;
+  BiasLong.RecommendedValues = { TEXT("0.0") };
+  BiasLong.bRestrictToRecommended = false;
+
+  // Altitude
+  FActorVariation StdDevAlt;
+  StdDevAlt.Id = TEXT("noise_alt_stddev");
+  StdDevAlt.Type = EActorAttributeType::Float;
+  StdDevAlt.RecommendedValues = { TEXT("0.0") };
+  StdDevAlt.bRestrictToRecommended = false;
+  FActorVariation BiasAlt;
+  BiasAlt.Id = TEXT("noise_alt_bias");
+  BiasAlt.Type = EActorAttributeType::Float;
+  BiasAlt.RecommendedValues = { TEXT("0.0") };
+  BiasAlt.bRestrictToRecommended = false;
+
+  Definition.Variations.Append({
+    StdDevLat,
+    BiasLat,
+    StdDevLong,
+    BiasLong,
+    StdDevAlt,
+    BiasAlt});
+
+  Success = CheckActorDefinition(Definition);
+}
+
 void UActorBlueprintFunctionLibrary::MakeVehicleDefinition(
     const FVehicleParameters &Parameters,
     bool &Success,
@@ -1203,4 +1266,26 @@ void UActorBlueprintFunctionLibrary::SetLidar(
       RetrieveActorAttributeToFloat("lower_fov", Description.Variations, Lidar.LowerFovLimit);
 }
 
+
+static void SetGnss(const FActorDescription &Description, AGnssSensor *Gnss);
+
+void UActorBlueprintFunctionLibrary::SetGnss(
+    const FActorDescription &Description,
+    AGnssSensor *Gnss)
+{
+
+  if(Gnss == nullptr) {
+    return;
+  }
+
+  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(RetrieveActorAttributeToFloat("noise_alt_stddev", Description.Variations, 0.0f))
+  Gnss->SetAltitudeBias(RetrieveActorAttributeToFloat("noise_alt_bias", Description.Variations, 0.0f))
+
+}
+
+
 #undef CARLA_ABFL_CHECK_ACTOR

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.h

@@ -11,6 +11,7 @@
 #include "Carla/Actor/PedestrianParameters.h"
 #include "Carla/Actor/PropParameters.h"
 #include "Carla/Actor/VehicleParameters.h"
+#include "Carla/Sensor/GnssSensor.h"
 
 #include "Kismet/BlueprintFunctionLibrary.h"
 
@@ -76,6 +77,13 @@ public:
       bool &Success,
       FActorDefinition &Definition);
 
+  static FActorDefinition MakeGnssDefinition();
+
+  UFUNCTION(Category = "Carla Actor", BlueprintCallable)
+  static void MakeGnssDefinition(
+      bool &Success,
+      FActorDefinition &Definition);
+
   UFUNCTION(Category = "Carla Actor", BlueprintCallable)
   static void MakeVehicleDefinition(
       const FVehicleParameters &Parameters,
@@ -189,5 +197,7 @@ public:
 
   static void SetLidar(const FActorDescription &Description, FLidarDescription &Lidar);
 
+  static void SetGnss(const FActorDescription &Description, AGnssSensor *Gnss);
+
   /// @}
 };

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.cpp

@@ -16,13 +16,12 @@ AGnssSensor::AGnssSensor(const FObjectInitializer &ObjectInitializer)
   : Super(ObjectInitializer)
 {
   PrimaryActorTick.bCanEverTick = true;
+  RandomEngine = CreateDefaultSubobject<URandomEngine>(TEXT("RandomEngine"));
 }
 
 FActorDefinition AGnssSensor::GetSensorDefinition()
 {
-  return UActorBlueprintFunctionLibrary::MakeGenericSensorDefinition(
-    TEXT("other"),
-    TEXT("gnss"));
+  return UActorBlueprintFunctionLibrary::MakeGnssDefinition();
 }
 
 void AGnssSensor::Tick(float DeltaSeconds)
@@ -34,13 +33,70 @@ void AGnssSensor::Tick(float DeltaSeconds)
   carla::geom::GeoLocation current_location = CurrentGeoLocation.Transform(location);
 
   auto Stream = GetDataStream(*this);
-  double latitude = current_location.latitude;
-  double longitude = current_location.longitude;
-  double altitude = current_location.altitude;
+  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);;
   Stream.Send(*this, carla::geom::GeoLocation{latitude, longitude, altitude});
 
 }
 
+void AGnssSensor::SetLatitudeDeviation(float Value)
+{
+  LatitudeDeviation = Value;
+}
+
+void AGnssSensor::SetLongitudeDeviation(float Value)
+{
+  LongitudeDeviation = Value;
+}
+
+void AGnssSensor::SetAltitudeDeviation(float Value)
+{
+  AltitudeDeviation = Value;
+}
+
+
+void AGnssSensor::SetLatitudeBias(float Value)
+{
+  LatitudeBias = Value;
+}
+
+void AGnssSensor::SetLongitudeBias(float Value)
+{
+  LongitudeBias = Value;
+}
+
+void AGnssSensor::SetAltitudeBias(float Value)
+{
+  AltitudeBias = Value;
+}
+
+float AGnssSensor::GetLatitudeDeviation() const
+{
+  return LatitudeDeviation;
+}
+float AGnssSensor::GetLongitudeDeviation() const
+{
+  return LongitudeDeviation;
+}
+float AGnssSensor::GetAltitudeDeviation() const
+{
+  return AltitudeDeviation;
+}
+
+float AGnssSensor::GetLatitudeBias() const
+{
+  return LatitudeBias;
+}
+float AGnssSensor::GetLongitudeBias() const
+{
+  return LongitudeBias;
+}
+float AGnssSensor::GetAltitudeBias() const
+{
+  return AltitudeBias;
+}
+
 void AGnssSensor::BeginPlay()
 {
   Super::BeginPlay();

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.h

@@ -32,6 +32,23 @@ public:
 
   void Tick(float DeltaSeconds) override;
 
+  void SetLatitudeDeviation(float Value);
+  void SetLongitudeDeviation(float Value);
+  void SetAltitudeDeviation(float Value);
+
+  void SetLatitudeBias(float Value);
+  void SetLongitudeBias(float Value);
+  void SetAltitudeBias(float Value);
+
+  float GetLatitudeDeviation() const;
+  float GetLongitudeDeviation() const;
+  float GetAltitudeDeviation() const;
+
+  float GetLatitudeBias() const;
+  float GetLongitudeBias() const;
+  float GetAltitudeBias() const;
+
+
 protected:
 
   virtual void BeginPlay() override;
@@ -41,4 +58,12 @@ private:
 
   carla::geom::GeoLocation CurrentGeoLocation;
 
+  float LatitudeDeviation;
+  float LongitudeDeviation;
+  float AltitudeDeviation;
+
+  float LatitudeBias;
+  float LongitudeBias;
+  float AltitudeBias;
+
 };

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Sensor.h

@@ -68,6 +68,9 @@ protected:
     return Stream.MakeAsyncDataStream(Self, GetEpisode().GetElapsedGameTime());
   }
 
+  UPROPÊRTY()
+  URandomEngine* RandomEngine;
+
 private:
 
   FDataStream Stream;