Add Gaussian noise to Odometry Publisher (#1393)

* Added gaussian noise and odometry with covariance publisher

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

Co-authored-by: Louise Poubel <[email protected]>

CMakeLists.txt

@@ -74,7 +74,7 @@ set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR})
 
 #--------------------------------------
 # Find ignition-msgs
-ign_find_package(ignition-msgs8 REQUIRED VERSION 8.2)
+ign_find_package(ignition-msgs8 REQUIRED VERSION 8.3)
 set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR})
 
 #--------------------------------------

src/systems/odometry_publisher/OdometryPublisher.cc

@@ -18,6 +18,7 @@
 #include "OdometryPublisher.hh"
 
 #include <ignition/msgs/odometry.pb.h>
+#include <ignition/msgs/odometry_with_covariance.pb.h>
 
 #include <limits>
 #include <string>
@@ -76,6 +77,9 @@ class ignition::gazebo::systems::OdometryPublisherPrivate
   /// \brief Diff drive odometry message publisher.
   public: transport::Node::Publisher odomPub;
 
+  /// \brief Diff drive odometry with covariance message publisher.
+  public: transport::Node::Publisher odomCovPub;
+
   /// \brief Rolling mean accumulators for the linear velocity
   public: std::tuple<math::RollingMean, math::RollingMean, math::RollingMean>
     linearMean;
@@ -95,6 +99,9 @@ class ignition::gazebo::systems::OdometryPublisherPrivate
 
   /// \brief Allow specifying constant xyz and rpy offsets
   public: ignition::math::Pose3d offset = {0, 0, 0, 0, 0, 0};
+
+  /// \brief Gaussian noise
+  public: double gaussianNoise = 0.0;
 };
 
 //////////////////////////////////////////////////
@@ -158,6 +165,11 @@ void OdometryPublisher::Configure(const Entity &_entity,
         "rpy_offset"));
   }
 
+  if (_sdf->HasElement("gaussian_noise"))
+  {
+    this->dataPtr->gaussianNoise = _sdf->Get<double>("gaussian_noise");
+  }
+
   this->dataPtr->robotBaseFrame = this->dataPtr->model.Name(_ecm)
     + "/" + "base_footprint";
   if (!_sdf->HasElement("robot_base_frame"))
@@ -199,17 +211,38 @@ void OdometryPublisher::Configure(const Entity &_entity,
   // Setup odometry
   std::string odomTopic{"/model/" + this->dataPtr->model.Name(_ecm) +
     "/odometry"};
+  std::string odomCovTopic{"/model/" + this->dataPtr->model.Name(_ecm) +
+    "/odometry_with_covariance"};
+
   if (_sdf->HasElement("odom_topic"))
     odomTopic = _sdf->Get<std::string>("odom_topic");
+  if (_sdf->HasElement("odom_covariance_topic"))
+    odomCovTopic = _sdf->Get<std::string>("odom_covariance_topic");
+
   std::string odomTopicValid {transport::TopicUtils::AsValidTopic(odomTopic)};
   if (odomTopicValid.empty())
   {
     ignerr << "Failed to generate odom topic ["
            << odomTopic << "]" << std::endl;
-    return;
   }
-  this->dataPtr->odomPub = this->dataPtr->node.Advertise<msgs::Odometry>(
-      odomTopicValid);
+  else
+  {
+    this->dataPtr->odomPub = this->dataPtr->node.Advertise<msgs::Odometry>(
+        odomTopicValid);
+  }
+
+  std::string odomCovTopicValid {
+    transport::TopicUtils::AsValidTopic(odomCovTopic)};
+  if (odomCovTopicValid.empty())
+  {
+    ignerr << "Failed to generate odom topic ["
+           << odomCovTopic << "]" << std::endl;
+  }
+  else
+  {
+    this->dataPtr->odomCovPub = this->dataPtr->node.Advertise<
+        msgs::OdometryWithCovariance>(odomCovTopicValid);
+  }
 }
 
 //////////////////////////////////////////////////
@@ -303,9 +336,18 @@ 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));
+    msg.mutable_twist()->mutable_linear()->set_z(
+      ignition::math::Rand::DblNormal(0, this->gaussianNoise));
+
+    msg.mutable_twist()->mutable_angular()->set_x(
+      ignition::math::Rand::DblNormal(0, this->gaussianNoise));
+    msg.mutable_twist()->mutable_angular()->set_y(
+      ignition::math::Rand::DblNormal(0, this->gaussianNoise));
   }
   // Get velocities and roll/pitch rates assuming 3D
   else if (this->dimensions == 3)
@@ -334,21 +376,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(
@@ -373,7 +421,73 @@ void OdometryPublisherPrivate::UpdateOdometry(
     return;
   }
   this->lastOdomPubTime = _info.simTime;
-  this->odomPub.Publish(msg);
+  if (this->odomPub.Valid())
+  {
+    this->odomPub.Publish(msg);
+  }
+
+  // Generate odometry with covariance message and publish it.
+  msgs::OdometryWithCovariance msgCovariance;
+
+  // Set the time stamp in the header.
+  msgCovariance.mutable_header()->mutable_stamp()->CopyFrom(
+      convert<msgs::Time>(_info.simTime));
+
+  // Set the frame ids.
+  frame = msgCovariance.mutable_header()->add_data();
+  frame->set_key("frame_id");
+  frame->add_value(odomFrame);
+  childFrame = msg.mutable_header()->add_data();
+  childFrame->set_key("child_frame_id");
+  childFrame->add_value(robotBaseFrame);
+
+  // Copy position from odometry msg.
+  msgCovariance.mutable_pose_with_covariance()->
+    mutable_pose()->mutable_position()->set_x(msg.pose().position().x());
+  msgCovariance.mutable_pose_with_covariance()->
+    mutable_pose()->mutable_position()->set_y(msg.pose().position().y());
+  msgCovariance.mutable_pose_with_covariance()->
+    mutable_pose()->mutable_position()->set_z(msg.pose().position().z());
+
+  // Copy twist from odometry msg.
+  msgCovariance.mutable_twist_with_covariance()->
+    mutable_twist()->mutable_angular()->set_x(msg.twist().angular().x());
+  msgCovariance.mutable_twist_with_covariance()->
+    mutable_twist()->mutable_angular()->set_y(msg.twist().angular().y());
+  msgCovariance.mutable_twist_with_covariance()->
+    mutable_twist()->mutable_angular()->set_z(msg.twist().angular().z());
+
+  msgCovariance.mutable_twist_with_covariance()->
+    mutable_twist()->mutable_linear()->set_x(msg.twist().linear().x());
+  msgCovariance.mutable_twist_with_covariance()->
+    mutable_twist()->mutable_linear()->set_y(msg.twist().linear().y());
+  msgCovariance.mutable_twist_with_covariance()->
+    mutable_twist()->mutable_linear()->set_z(msg.twist().linear().z());
+
+  // Populate the covariance matrix.
+  // Should the matrix me populated for pose as well ?
+  auto gn2 = this->gaussianNoise * this->gaussianNoise;
+  for (int i = 0; i < 36; i++)
+  {
+    if (i % 7 == 0)
+    {
+      msgCovariance.mutable_pose_with_covariance()->
+        mutable_covariance()->add_data(gn2);
+      msgCovariance.mutable_twist_with_covariance()->
+        mutable_covariance()->add_data(gn2);
+    }
+    else
+    {
+      msgCovariance.mutable_pose_with_covariance()->
+        mutable_covariance()->add_data(0);
+      msgCovariance.mutable_twist_with_covariance()->
+        mutable_covariance()->add_data(0);
+    }
+  }
+  if (this->odomCovPub.Valid())
+  {
+    this->odomCovPub.Publish(msgCovariance);
+  }
 }
 
 IGNITION_ADD_PLUGIN(OdometryPublisher,

src/systems/odometry_publisher/OdometryPublisher.hh

@@ -53,6 +53,10 @@ namespace systems
   /// messages. This element is optional, and the default value is
   /// `/model/{name_of_model}/odometry`.
   ///
+  /// `<odom_covariance_topic>`: Custom topic on which this system will publish
+  /// odometry with covariance messages. This element is optional, and the
+  /// default value is `/model/{name_of_model}/odometry_with_covariance`.
+  ///
   /// `<dimensions>`: Number of dimensions to represent odometry. Only 2 and 3
   /// dimensional spaces are supported. This element is optional, and the
   /// default value is 2.
@@ -63,7 +67,11 @@ namespace systems
   ///
   /// `<rpy_offset>`: Rotation offset relative to the body fixed frame, the
   /// default value is 0 0 0. This offset will be added to the odometry
-  //  message.
+  ///  message.
+  ///
+  /// `<gaussian_noise>`: Standard deviation of the Gaussian noise to be added
+  /// to pose and twist messages. This element is optional, and the default
+  /// value is 0.
   class OdometryPublisher
       : public System,
         public ISystemConfigure,

test/integration/odometry_publisher.cc

@@ -440,6 +440,111 @@ class OdometryPublisherTest
     EXPECT_NEAR(lastPose.Rot().Pitch(), 0, 1e-2);
     EXPECT_NEAR(lastPose.Rot().Yaw(), 0, 1e-2);
   }
+
+  /// \param[in] _sdfFile SDF file to load.
+  /// \param[in] _odomTopic Odometry topic.
+  protected: void TestGaussianNoise(const std::string &_sdfFile,
+                               const std::string &_odomTopic)
+  {
+    // Start server
+    ServerConfig serverConfig;
+    serverConfig.SetSdfFile(_sdfFile);
+
+    Server server(serverConfig);
+    EXPECT_FALSE(server.Running());
+    EXPECT_FALSE(*server.Running(0));
+
+    std::vector<math::Vector3d> odomLinVels;
+    std::vector<math::Vector3d> odomAngVels;
+    google::protobuf::RepeatedField<float> odomTwistCovariance;
+    // Create function to store data from odometry messages
+    std::function<void(const msgs::OdometryWithCovariance &)> odomCb =
+      [&](const msgs::OdometryWithCovariance &_msg)
+      {
+        odomLinVels.push_back(msgs::Convert(_msg.twist_with_covariance().
+          twist().linear()));
+        odomAngVels.push_back(msgs::Convert(_msg.twist_with_covariance().
+          twist().angular()));
+        odomTwistCovariance = _msg.twist_with_covariance().covariance().data();
+      };
+    transport::Node node;
+    node.Subscribe(_odomTopic, odomCb);
+
+    // Run server while the model moves with the velocities set earlier
+    server.Run(true, 3000, false);
+
+    int sleep = 0;
+    int maxSleep = 30;
+    for (; odomLinVels.size() < 500 && sleep < maxSleep; ++sleep)
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+
+    // Verify the Gaussian noise.
+    ASSERT_FALSE(odomLinVels.empty());
+    ASSERT_FALSE(odomAngVels.empty());
+    int n = odomLinVels.size();
+
+    // Calculate the means.
+    double linVelSumX = 0, linVelSumY = 0, linVelSumZ = 0;
+    double angVelSumX = 0, angVelSumY = 0, angVelSumZ = 0;
+    for (int i = 0; i < n; i++)
+    {
+      linVelSumX += odomLinVels[i].X();
+      linVelSumY += odomLinVels[i].Y();
+      linVelSumZ += odomLinVels[i].Z();
+
+      angVelSumX += odomAngVels[i].X();
+      angVelSumY += odomAngVels[i].Y();
+      angVelSumZ += odomAngVels[i].Z();
+    }
+
+    // Check that the mean values are close to zero.
+    EXPECT_NEAR(linVelSumX/n, 0, 0.3);
+    EXPECT_NEAR(linVelSumY/n, 0, 0.3);
+    EXPECT_NEAR(linVelSumZ/n, 0, 0.3);
+
+    EXPECT_NEAR(angVelSumX/n, 0, 0.3);
+    EXPECT_NEAR(angVelSumY/n, 0, 0.3);
+    EXPECT_NEAR(angVelSumZ/n, 0, 0.3);
+
+    // Calculate the variation (sigma^2).
+    double linVelSqSumX = 0, linVelSqSumY = 0, linVelSqSumZ = 0;
+    double angVelSqSumX = 0, angVelSqSumY = 0, angVelSqSumZ = 0;
+    for (int i = 0; i < n; i++)
+    {
+      linVelSqSumX += std::pow(odomLinVels[i].X() - linVelSumX/n, 2);
+      linVelSqSumY += std::pow(odomLinVels[i].Y() - linVelSumY/n, 2);
+      linVelSqSumZ += std::pow(odomLinVels[i].Z() - linVelSumZ/n, 2);
+
+      angVelSqSumX += std::pow(odomAngVels[i].X() - angVelSumX/n, 2);
+      angVelSqSumY += std::pow(odomAngVels[i].Y() - angVelSumY/n, 2);
+      angVelSqSumZ += std::pow(odomAngVels[i].Z() - angVelSumZ/n, 2);
+    }
+
+    // Verify the variance values.
+    EXPECT_NEAR(linVelSqSumX/n, 1, 0.3);
+    EXPECT_NEAR(linVelSqSumY/n, 1, 0.3);
+    EXPECT_NEAR(linVelSqSumZ/n, 1, 0.3);
+
+    EXPECT_NEAR(angVelSqSumX/n, 1, 0.3);
+    EXPECT_NEAR(angVelSqSumY/n, 1, 0.3);
+    EXPECT_NEAR(angVelSqSumZ/n, 1, 0.3);
+
+    // Check the covariance matrix.
+    EXPECT_EQ(odomTwistCovariance.size(), 36);
+    for (int i = 0; i < 36; i++)
+    {
+      if (i % 7 == 0)
+      {
+        EXPECT_NEAR(odomTwistCovariance.Get(i), 1, 1e-2);
+      }
+      else
+      {
+        EXPECT_NEAR(odomTwistCovariance.Get(i), 0, 1e-2);
+      }
+    }
+  }
 };
 
 /////////////////////////////////////////////////
@@ -502,6 +607,16 @@ TEST_P(OdometryPublisherTest,
       "/model/vehicle/odometry");
 }
 
+/////////////////////////////////////////////////
+TEST_P(OdometryPublisherTest,
+       IGN_UTILS_TEST_DISABLED_ON_WIN32(GaussianNoiseTest))
+{
+  TestGaussianNoise(
+      std::string(PROJECT_SOURCE_PATH) +
+      "/test/worlds/odometry_noise.sdf",
+      "/model/vehicle/odometry_with_covariance");
+}
+
 // Run multiple times
 INSTANTIATE_TEST_SUITE_P(ServerRepeat, OdometryPublisherTest,
     ::testing::Range(1, 2));