diff --git a/include/gz/sensors/SensorTypes.hh b/include/gz/sensors/SensorTypes.hh index 5e096c34..fbdf6089 100644 --- a/include/gz/sensors/SensorTypes.hh +++ b/include/gz/sensors/SensorTypes.hh @@ -209,6 +209,18 @@ namespace gz /// \sa AirSpeedSensor AIR_SPEED_NOISE_PASCALS = 25, + /// \brief Orientation body-frame X axis (Roll) noise in rad + /// \sa ImuSensor + ORIENTATION_X_NOISE_RAD = 26, + + /// \brief Orientation body-frame Y axis (Pitch) noise in rad + /// \sa ImuSensor + ORIENTATION_Y_NOISE_RAD = 27, + + /// \brief Orientation body-frame Z axis (Yaw) noise in rad + /// \sa ImuSensor + ORIENTATION_Z_NOISE_RAD = 28, + /// \internal /// \brief Indicator used to create an iterator over the enum. Do not /// use this. diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index b38af199..a2c706fc 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -157,6 +157,9 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) {GYROSCOPE_X_NOISE_RAD_S, _sdf.ImuSensor()->AngularVelocityXNoise()}, {GYROSCOPE_Y_NOISE_RAD_S, _sdf.ImuSensor()->AngularVelocityYNoise()}, {GYROSCOPE_Z_NOISE_RAD_S, _sdf.ImuSensor()->AngularVelocityZNoise()}, + {ORIENTATION_X_NOISE_RAD, _sdf.ImuSensor()->OrientationXNoise()}, + {ORIENTATION_Y_NOISE_RAD, _sdf.ImuSensor()->OrientationYNoise()}, + {ORIENTATION_Z_NOISE_RAD, _sdf.ImuSensor()->OrientationZNoise()}, }; for (const auto & [noiseType, noiseSdf] : noises) @@ -288,9 +291,12 @@ bool ImuSensor::Update(const std::chrono::steady_clock::duration &_now) 4, getCov(GYROSCOPE_Y_NOISE_RAD_S)); msg.mutable_angular_velocity_covariance()->set_data( 8, getCov(GYROSCOPE_Z_NOISE_RAD_S)); - msg.mutable_orientation_covariance()->set_data(0, 0.0); - msg.mutable_orientation_covariance()->set_data(4, 0.0); - msg.mutable_orientation_covariance()->set_data(8, 0.0); + msg.mutable_orientation_covariance()->set_data( + 0, getCov(ORIENTATION_X_NOISE_RAD)); + msg.mutable_orientation_covariance()->set_data( + 4, getCov(ORIENTATION_Y_NOISE_RAD)); + msg.mutable_orientation_covariance()->set_data( + 8, getCov(ORIENTATION_Z_NOISE_RAD)); } if (this->dataPtr->orientationEnabled) @@ -301,7 +307,15 @@ bool ImuSensor::Update(const std::chrono::steady_clock::duration &_now) this->dataPtr->orientationReference.Inverse() * this->dataPtr->worldPose.Rot(); + // Adding noise to orientation + math::Vector3d euler = this->dataPtr->orientation.Euler(); + applyNoise(ORIENTATION_X_NOISE_RAD, euler.X()); + applyNoise(ORIENTATION_Y_NOISE_RAD, euler.Y()); + applyNoise(ORIENTATION_Z_NOISE_RAD, euler.Z()); + this->dataPtr->orientation.SetFromEuler(euler); + msgs::Set(msg.mutable_orientation(), this->dataPtr->orientation); + } msgs::Set(msg.mutable_angular_velocity(), this->dataPtr->angularVel); msgs::Set(msg.mutable_linear_acceleration(), this->dataPtr->linearAcc);