From 98442c9d110d34c0d9bcd6e35ef0b8d48dc72ba8 Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 22 Feb 2022 11:58:16 -0800 Subject: [PATCH] Added gaussian noise Signed-off-by: Aditya --- .../odometry_publisher/OdometryPublisher.cc | 36 ++++++++++++++----- 1 file changed, 28 insertions(+), 8 deletions(-) diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 5d6720b0895..0191b0c061f 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/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("rpy_offset")); } + if (!_sdf->HasElement("gaussian_noise")) + { + this->dataPtr->gaussianNoise = 0; + } + else + { + this->dataPtr->gaussianNoise = _sdf->Get("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(