Added gaussian noise

Signed-off-by: Aditya <[email protected]>

src/systems/odometry_publisher/OdometryPublisher.cc

@@ -95,6 +95,9 @@ class ignition::gazebo::systems::OdometryPublisherPrivate
 
   /// \brief Allow specifying constant xyz and rpy offsets
   public: ignition::math::Pose3d offset;
+
+  /// \brief Gaussian Noise
+  public: double gaussianNoise;
 };
 
 //////////////////////////////////////////////////
@@ -165,6 +168,15 @@ void OdometryPublisher::Configure(const Entity &_entity,
       ignition::math::Quaterniond(_sdf->Get<ignition::math::Vector3d>("rpy_offset"));
   }
 
+  if (!_sdf->HasElement("gaussian_noise"))
+  {
+    this->dataPtr->gaussianNoise = 0;
+  }
+  else
+  {
+    this->dataPtr->gaussianNoise = _sdf->Get<double>("gaussian_noise");
+  }
+
   this->dataPtr->robotBaseFrame = this->dataPtr->model.Name(_ecm)
     + "/" + "base_footprint";
   if (!_sdf->HasElement("robot_base_frame"))
@@ -312,9 +324,11 @@ void OdometryPublisherPrivate::UpdateOdometry(
     std::get<0>(this->linearMean).Push(linearVelocityX);
     std::get<1>(this->linearMean).Push(linearVelocityY);
     msg.mutable_twist()->mutable_linear()->set_x(
-      std::get<0>(this->linearMean).Mean());
+      std::get<0>(this->linearMean).Mean() +
+      ignition::math::Rand::DblNormal(0, this->gaussianNoise));
     msg.mutable_twist()->mutable_linear()->set_y(
-      std::get<1>(this->linearMean).Mean());
+      std::get<1>(this->linearMean).Mean() +
+      ignition::math::Rand::DblNormal(0, this->gaussianNoise));
   }
   // Get velocities and roll/pitch rates assuming 3D
   else if (this->dimensions == 3)
@@ -343,21 +357,27 @@ void OdometryPublisherPrivate::UpdateOdometry(
     std::get<0>(this->angularMean).Push(rollDiff / dt.count());
     std::get<1>(this->angularMean).Push(pitchDiff / dt.count());
     msg.mutable_twist()->mutable_linear()->set_x(
-      std::get<0>(this->linearMean).Mean());
+      std::get<0>(this->linearMean).Mean() +
+      ignition::math::Rand::DblNormal(0, this->gaussianNoise));
     msg.mutable_twist()->mutable_linear()->set_y(
-      std::get<1>(this->linearMean).Mean());
+      std::get<1>(this->linearMean).Mean() +
+      ignition::math::Rand::DblNormal(0, this->gaussianNoise));
     msg.mutable_twist()->mutable_linear()->set_z(
-      std::get<2>(this->linearMean).Mean());
+      std::get<2>(this->linearMean).Mean() +
+      ignition::math::Rand::DblNormal(0, this->gaussianNoise));
     msg.mutable_twist()->mutable_angular()->set_x(
-      std::get<0>(this->angularMean).Mean());
+      std::get<0>(this->angularMean).Mean() +
+      ignition::math::Rand::DblNormal(0, this->gaussianNoise));
     msg.mutable_twist()->mutable_angular()->set_y(
-      std::get<1>(this->angularMean).Mean());
+      std::get<1>(this->angularMean).Mean() +
+      ignition::math::Rand::DblNormal(0, this->gaussianNoise));
   }
 
   // Set yaw rate
   std::get<2>(this->angularMean).Push(yawDiff / dt.count());
   msg.mutable_twist()->mutable_angular()->set_z(
-    std::get<2>(this->angularMean).Mean());
+    std::get<2>(this->angularMean).Mean() +
+      ignition::math::Rand::DblNormal(0, this->gaussianNoise));
 
   // Set the time stamp in the header.
   msg.mutable_header()->mutable_stamp()->CopyFrom(